From 1c3da08b50c4a9703e08037a89766ed87e3c5e8b Mon Sep 17 00:00:00 2001 From: Billy Okal Date: Fri, 22 Aug 2014 19:55:14 +0200 Subject: [PATCH] minor tweaks --- .../pedsim_simulator/agentstatemachine.h | 10 +- .../include/pedsim_simulator/simulator.h | 152 ++++--- pedsim_simulator/src/element/agentgroup.cpp | 220 ++++----- pedsim_simulator/src/simulator.cpp | 425 +++++++++--------- .../individualwaypointplanner.cpp | 11 - .../src/waypointplanner/queueingplanner.cpp | 3 +- 6 files changed, 396 insertions(+), 425 deletions(-) diff --git a/pedsim_simulator/include/pedsim_simulator/agentstatemachine.h b/pedsim_simulator/include/pedsim_simulator/agentstatemachine.h index a255bea9..76703427 100644 --- a/pedsim_simulator/include/pedsim_simulator/agentstatemachine.h +++ b/pedsim_simulator/include/pedsim_simulator/agentstatemachine.h @@ -43,10 +43,12 @@ class GroupWaypointPlanner; class ShoppingPlanner; -class AgentStateMachine : public QObject { +class AgentStateMachine : public QObject +{ Q_OBJECT // Enums + // TODO - switch to enum classes public: typedef enum { StateNone = 0, @@ -56,24 +58,19 @@ class AgentStateMachine : public QObject { StateGroupWalking = 4, StateShopping = 5 } AgentState; - Q_ENUMS(AgentState) - // Constructor and Destructor public: AgentStateMachine(Agent* agentIn); virtual ~AgentStateMachine(); - // Signals signals: void stateChanged(AgentState newState); - public slots: void loseAttraction(); - // Methods public: void doStateTransition(); @@ -84,7 +81,6 @@ public slots: bool checkGroupForAttractions(AttractionArea** attractionOut = nullptr) const; QString stateToName(AgentState stateIn) const; - // Attributes protected: Agent* agent; diff --git a/pedsim_simulator/include/pedsim_simulator/simulator.h b/pedsim_simulator/include/pedsim_simulator/simulator.h index 8d218bf7..7ed292f2 100644 --- a/pedsim_simulator/include/pedsim_simulator/simulator.h +++ b/pedsim_simulator/include/pedsim_simulator/simulator.h @@ -80,51 +80,51 @@ class Simulator { public: - Simulator(const ros::NodeHandle &node); - virtual ~Simulator(); - - bool initializeSimulation(); - void loadConfigParameters(); - void runSimulation(); - - /// publishers - void publishAgents(); - void publishData(); - void publishSocialActivities(); - void publishGroupVisuals(); - void publishObstacles(); - void publishWalls(); - void publishAttractions(); - - /// subscriber helpers - // Drive robot based on topic messages - void callbackRobotCommand ( const pedsim_msgs::AgentState::ConstPtr &msg ); + Simulator(const ros::NodeHandle &node); + virtual ~Simulator(); + + bool initializeSimulation(); + void loadConfigParameters(); + void runSimulation(); + + /// publishers + void publishAgents(); + void publishData(); + void publishSocialActivities(); + void publishGroupVisuals(); + void publishObstacles(); + void publishWalls(); + void publishAttractions(); + + /// subscriber helpers + // Drive robot based on topic messages + void callbackRobotCommand(const pedsim_msgs::AgentState::ConstPtr &msg); private: - // robot agent - Agent* robot_; - - ros::NodeHandle nh_; - - /// publishers - // - data messages - ros::Publisher pub_obstacles_; // grid cells - ros::Publisher pub_all_agents_; // positions and velocities (old msg) - ros::Publisher pub_tracked_persons_; // in spencer format - ros::Publisher pub_tracked_groups_; - ros::Publisher pub_social_activities_; - - // - visualization related messages (e.g. markers) - ros::Publisher pub_attractions_; - ros::Publisher pub_agent_visuals_; - ros::Publisher pub_group_lines_; - ros::Publisher pub_walls_; - ros::Publisher pub_queues_; - ros::Publisher pub_waypoints_; - ros::Publisher pub_agent_arrows_; - - /// subscribers + // robot agent + Agent *robot_; + + ros::NodeHandle nh_; + + /// publishers + // - data messages + ros::Publisher pub_obstacles_; // grid cells + ros::Publisher pub_all_agents_; // positions and velocities (old msg) + ros::Publisher pub_tracked_persons_; // in spencer format + ros::Publisher pub_tracked_groups_; + ros::Publisher pub_social_activities_; + + // - visualization related messages (e.g. markers) + ros::Publisher pub_attractions_; + ros::Publisher pub_agent_visuals_; + ros::Publisher pub_group_lines_; + ros::Publisher pub_walls_; + ros::Publisher pub_queues_; + ros::Publisher pub_waypoints_; + ros::Publisher pub_agent_arrows_; + + /// subscribers ros::Subscriber sub_robot_command_; // - Covenient object to handling quaternions @@ -132,42 +132,40 @@ class Simulator private: - inline Eigen::Quaternionf computePose( Agent* a ) - { - double theta = atan2 ( a->getvy(), a->getvx() ); - double aa = M_PI / 2.0; - double b = 0.0; - double c = theta + (M_PI / 2.0); - - Eigen::Quaternionf q = orientation_handler_->rpy2Quaternion(aa, c, b); - return q; - } - - inline std::string agentStateToActivity( AgentStateMachine::AgentState state ) - { - std::string activity = "Unknown"; - - switch ( state ) - { - case AgentStateMachine::AgentState::StateWalking: - activity = pedsim_msgs::AgentState::TYPE_INDIVIDUAL_MOVING; - break; - case AgentStateMachine::AgentState::StateGroupWalking: - activity = pedsim_msgs::AgentState::TYPE_GROUP_MOVING; - break; - case AgentStateMachine::AgentState::StateQueueing: - activity = pedsim_msgs::AgentState::TYPE_WAITING_IN_QUEUE; - break; - case AgentStateMachine::AgentState::StateShopping: - activity = pedsim_msgs::AgentState::TYPE_SHOPPING; - break; + /// \brief Compute pose of an agent in quaternion format + inline Eigen::Quaternionf computePose(Agent *a) { + double theta = atan2(a->getvy(), a->getvx()); + Eigen::Quaternionf q = orientation_handler_->rpy2Quaternion(M_PI / 2.0, theta + (M_PI / 2.0), 0.0); + return q; + } + + inline std::string agentStateToActivity(AgentStateMachine::AgentState state) { + std::string activity = "Unknown"; + + switch (state) { + case AgentStateMachine::AgentState::StateWalking: + activity = pedsim_msgs::AgentState::TYPE_INDIVIDUAL_MOVING; + break; + case AgentStateMachine::AgentState::StateGroupWalking: + activity = pedsim_msgs::AgentState::TYPE_GROUP_MOVING; + break; + case AgentStateMachine::AgentState::StateQueueing: + activity = pedsim_msgs::AgentState::TYPE_WAITING_IN_QUEUE; + break; + case AgentStateMachine::AgentState::StateShopping: + activity = pedsim_msgs::AgentState::TYPE_SHOPPING; + break; + case AgentStateMachine::AgentState::StateNone: + break; + case AgentStateMachine::AgentState::StateWaiting: + break; } - // TODO - // - add standing to the state machine - // - add waiting at the end of the queue - // - add group walking + // TODO + // - add standing to the state machine + // - add waiting at the end of the queue + // - add group walking - return activity; - } + return activity; + } }; diff --git a/pedsim_simulator/src/element/agentgroup.cpp b/pedsim_simulator/src/element/agentgroup.cpp index a99c3956..13baf91b 100644 --- a/pedsim_simulator/src/element/agentgroup.cpp +++ b/pedsim_simulator/src/element/agentgroup.cpp @@ -45,15 +45,15 @@ AgentGroup::AgentGroup() dirtyMaxDistance = true; recollecting = true; // → delayed center of mass update - comUpdateTimer.setSingleShot ( true ); - comUpdateTimer.setInterval ( 0 ); - connect ( &comUpdateTimer, SIGNAL ( timeout() ), this, SLOT ( updateCenterOfMass() ) ); + comUpdateTimer.setSingleShot(true); + comUpdateTimer.setInterval(0); + connect(&comUpdateTimer, SIGNAL(timeout()), this, SLOT(updateCenterOfMass())); // compute center of mass updateCenterOfMass(); } -AgentGroup::AgentGroup ( const QList& agentsIn ) +AgentGroup::AgentGroup(const QList &agentsIn) { static int staticid = 0; id_ = staticid++; @@ -63,20 +63,20 @@ AgentGroup::AgentGroup ( const QList& agentsIn ) dirtyMaxDistance = true; members = agentsIn; // → delayed center of mass update - comUpdateTimer.setSingleShot ( false ); - comUpdateTimer.setInterval ( 0 ); - connect ( &comUpdateTimer, SIGNAL ( timeout() ), this, SLOT ( updateCenterOfMass() ) ); + comUpdateTimer.setSingleShot(false); + comUpdateTimer.setInterval(0); + connect(&comUpdateTimer, SIGNAL(timeout()), this, SLOT(updateCenterOfMass())); // compute center of mass updateCenterOfMass(); // connect signals - foreach ( Agent* agent, members ) - connect ( agent, SIGNAL ( positionChanged ( double,double ) ), - this, SLOT ( onPositionChanged ( double,double ) ) ); + foreach (Agent * agent, members) + connect(agent, SIGNAL(positionChanged(double, double)), + this, SLOT(onPositionChanged(double, double))); } -AgentGroup::AgentGroup ( std::initializer_list& agentsIn ) +AgentGroup::AgentGroup(std::initializer_list &agentsIn) { static int staticid = 0; id_ = staticid++; @@ -84,21 +84,19 @@ AgentGroup::AgentGroup ( std::initializer_list& agentsIn ) // initialize values dirty = true; dirtyMaxDistance = true; - comUpdateTimer.setSingleShot ( true ); - comUpdateTimer.setInterval ( 0 ); - connect ( &comUpdateTimer, SIGNAL ( timeout() ), this, SLOT ( updateCenterOfMass() ) ); + comUpdateTimer.setSingleShot(true); + comUpdateTimer.setInterval(0); + connect(&comUpdateTimer, SIGNAL(timeout()), this, SLOT(updateCenterOfMass())); // add agents from initializer_list to the member list - for ( Agent* currentAgent : agentsIn ) - { - members.append ( currentAgent ); - connect ( currentAgent, SIGNAL ( positionChanged ( double,double ) ), - this, SLOT ( onPositionChanged ( double,double ) ) ); + for (Agent * currentAgent : agentsIn) { + members.append(currentAgent); + connect(currentAgent, SIGNAL(positionChanged(double, double)), + this, SLOT(onPositionChanged(double, double))); } // compute center of mass updateCenterOfMass(); - } AgentGroup::~AgentGroup() @@ -106,7 +104,7 @@ AgentGroup::~AgentGroup() } -void AgentGroup::onPositionChanged ( double x, double y ) +void AgentGroup::onPositionChanged(double x, double y) { // mark center of mass as dirty (needs to be re-calculated) dirty = true; @@ -114,101 +112,92 @@ void AgentGroup::onPositionChanged ( double x, double y ) comUpdateTimer.start(); } -QList AgentGroup::divideAgents ( const QList& agentsIn ) +QList AgentGroup::divideAgents(const QList &agentsIn) { - QList groups; - QList unassignedAgents = agentsIn; + QList groups; + QList unassignedAgents = agentsIn; // initialize Poisson distribution - std::poisson_distribution distribution ( CONFIG.group_size_lambda ); + std::poisson_distribution distribution(CONFIG.group_size_lambda); // distribution of group sizes QVector sizeDistribution; // → create distribution int agentCount = agentsIn.count(); int sizeSum = 0; - while ( sizeSum < agentCount ) - { + while (sizeSum < agentCount) { // randomly draw the group size (Poisson distribution) // (don't use group size = 0) int groupSize; - do - { - groupSize = distribution ( RNG() ); - } - while ( groupSize == 0 ); + do { + groupSize = distribution(RNG()); + } while (groupSize == 0); // → limit group size to the number of agents left - groupSize = min ( groupSize, agentCount-sizeSum ); + groupSize = min(groupSize, agentCount - sizeSum); // → record group size - if ( sizeDistribution.size() < groupSize ) - sizeDistribution.resize ( groupSize ); - sizeDistribution[groupSize-1]++; + if (sizeDistribution.size() < groupSize) + sizeDistribution.resize(groupSize); + sizeDistribution[groupSize - 1]++; // → update sum over all group sizes sizeSum += groupSize; } // → report group size distribution - reportSizeDistribution ( sizeDistribution ); + reportSizeDistribution(sizeDistribution); - if ( CONFIG.groups_enabled ) - { + if (CONFIG.groups_enabled) { // → iterate over all group sizes and create groups accordingly // (start with the largest size to receive contiguous groups) - for ( int groupSize = sizeDistribution.count(); groupSize > 0; --groupSize ) - { + for (int groupSize = sizeDistribution.count(); groupSize > 0; --groupSize) { // create groups of given size - for ( int groupIter = 0; groupIter < sizeDistribution[groupSize-1]; ++groupIter ) - { - Agent* groupLeader = unassignedAgents.takeFirst(); - - /// old people are not allowed to lead - /// other leaders of tomorrow will never be a reality - if (groupLeader->getType() == Ped::Tagent::ELDER) - continue; - + for (int groupIter = 0; groupIter < sizeDistribution[groupSize - 1]; ++groupIter) { + Agent *groupLeader = unassignedAgents.takeFirst(); + + /// old people are not allowed to lead + /// other leaders of tomorrow will never be a reality + if (groupLeader->getType() == Ped::Tagent::ELDER) + continue; + // create a group - AgentGroup* newGroup = new AgentGroup(); + AgentGroup *newGroup = new AgentGroup(); // and add it to result set - groups.append ( newGroup ); + groups.append(newGroup); // add first agent to the group Ped::Tvector leaderPosition = groupLeader->getPosition(); - newGroup->addMember ( groupLeader ); + newGroup->addMember(groupLeader); // add other agents to group - QList > distanceList; - foreach ( Agent* potentialMember, unassignedAgents ) - { + QList > distanceList; + foreach (Agent * potentialMember, unassignedAgents) { Ped::Tvector position = potentialMember->getPosition(); - double distance = ( leaderPosition - position ).length(); + double distance = (leaderPosition - position).length(); // add potential group member to the list according to the distance auto iter = distanceList.begin(); - while ( iter < distanceList.end() ) - { - if ( distance > iter->second ) + while (iter < distanceList.end()) { + if (distance > iter->second) break; else ++iter; } // → insert candidate - distanceList.insert ( iter, qMakePair ( potentialMember, distance ) ); + distanceList.insert(iter, qMakePair(potentialMember, distance)); // reduce list if necessary - if ( distanceList.size() > groupSize-1 ) + if (distanceList.size() > groupSize - 1) distanceList.removeFirst(); } // add neighbors to the group - foreach ( const auto& member, distanceList ) - { - newGroup->addMember ( member.first ); + foreach (const auto & member, distanceList) { + newGroup->addMember(member.first); // don't consider the group member as part of another group - unassignedAgents.removeOne ( member.first ); + unassignedAgents.removeOne(member.first); } } } @@ -218,50 +207,48 @@ QList AgentGroup::divideAgents ( const QList& agentsIn ) return groups; } -QList& AgentGroup::getMembers() +QList &AgentGroup::getMembers() { return members; } -const QList& AgentGroup::getMembers() const +const QList &AgentGroup::getMembers() const { return members; } -bool AgentGroup::addMember ( Agent* agentIn ) +bool AgentGroup::addMember(Agent *agentIn) { - if ( members.contains ( agentIn ) ) - { - ROS_DEBUG("AgentGroup: Couldn't add Agent twice!"); + if (members.contains(agentIn)) { + ROS_DEBUG("AgentGroup: Couldn't add Agent twice!"); return false; } // add Agent to the group and mark cache invalid - members.append ( agentIn ); + members.append(agentIn); dirty = true; dirtyMaxDistance = true; comUpdateTimer.start(); // connect signals - connect ( agentIn, SIGNAL ( positionChanged ( double,double ) ), - this, SLOT ( onPositionChanged ( double,double ) ) ); + connect(agentIn, SIGNAL(positionChanged(double, double)), + this, SLOT(onPositionChanged(double, double))); // inform users - emit memberAdded ( agentIn->getId() ); + emit memberAdded(agentIn->getId()); return true; } -bool AgentGroup::removeMember ( Agent* agentIn ) +bool AgentGroup::removeMember(Agent *agentIn) { - bool hasRemovedMember = members.removeOne ( agentIn ); + bool hasRemovedMember = members.removeOne(agentIn); // mark cache invalid, if the agent has been removed - if ( hasRemovedMember == true ) - { + if (hasRemovedMember == true) { // disconnect signals - disconnect ( agentIn, SIGNAL ( positionChanged ( double,double ) ), - this, SLOT ( onPositionChanged ( double,double ) ) ); + disconnect(agentIn, SIGNAL(positionChanged(double, double)), + this, SLOT(onPositionChanged(double, double))); // invalidate cache and schedule update dirty = true; @@ -269,17 +256,15 @@ bool AgentGroup::removeMember ( Agent* agentIn ) comUpdateTimer.start(); // inform users - emit memberRemoved ( agentIn->getId() ); + emit memberRemoved(agentIn->getId()); return true; - } - else - { + } else { return false; } } -bool AgentGroup::setMembers ( const QList& agentsIn ) +bool AgentGroup::setMembers(const QList &agentsIn) { // set the new members and mark cache invalid members = agentsIn; @@ -288,11 +273,12 @@ bool AgentGroup::setMembers ( const QList& agentsIn ) comUpdateTimer.start(); // connect signals - foreach ( Agent* agent, members ) - connect ( agent, SIGNAL ( positionChanged ( double,double ) ), - this, SLOT ( onPositionChanged ( double,double ) ) ); + foreach (Agent * agent, members) + connect(agent, SIGNAL(positionChanged(double, double)), + this, SLOT(onPositionChanged(double, double))); // inform users + // TODO - we need to get away from using signals emit membersChanged(); return true; @@ -311,10 +297,9 @@ int AgentGroup::memberCount() const Ped::Tvector AgentGroup::getCenterOfMass() const { // check cache - if ( dirty ) - { + if (dirty) { // update cache - AgentGroup* nonConstThis = const_cast ( this ); + AgentGroup *nonConstThis = const_cast(this); nonConstThis->updateCenterOfMass(); } @@ -323,15 +308,14 @@ Ped::Tvector AgentGroup::getCenterOfMass() const Ped::Tvector AgentGroup::updateCenterOfMass() { - if ( !dirty ) + if (!dirty) return cacheCoM; // compute center of mass Ped::Tvector com; - foreach ( const Agent* member, members ) - { + foreach (const Agent * member, members) { com += member->getPosition(); - } + } int groupSize = members.size(); com /= groupSize; @@ -345,24 +329,21 @@ Ped::Tvector AgentGroup::updateCenterOfMass() return cacheCoM; } -void AgentGroup::setRecollect ( bool recollectIn ) +void AgentGroup::setRecollect(bool recollectIn) { - if ( recollectIn ) - { + if (recollectIn) { // check whether recollecting mode has already been activated - if ( recollecting ) + if (recollecting) return; - ROS_DEBUG("AgentGroup needs to recollect! (%s)", toString().toStdString().c_str()); + ROS_DEBUG("AgentGroup needs to recollect! (%s)", toString().toStdString().c_str()); recollecting = true; - } - else - { + } else { // check whether recollecting mode hasn't been activated - if ( !recollecting ) + if (!recollecting) return; - ROS_DEBUG("AgentGroup finished recollecting! (%s)", toString().toStdString().c_str()); + ROS_DEBUG("AgentGroup finished recollecting! (%s)", toString().toStdString().c_str()); recollecting = false; } } @@ -374,7 +355,7 @@ bool AgentGroup::isRecollecting() const double AgentGroup::getMaxDistance() { - if ( dirty || dirtyMaxDistance ) + if (dirty || dirtyMaxDistance) updateMaxDistance(); return cacheMaxDistance; @@ -384,39 +365,36 @@ void AgentGroup::updateMaxDistance() { Ped::Tvector com = getCenterOfMass(); double maxDistance = 0; - foreach ( Agent* agent, members ) - { - double distance = ( com - agent->getPosition() ).length(); - if ( distance > maxDistance ) + foreach (Agent * agent, members) { + double distance = (com - agent->getPosition()).length(); + if (distance > maxDistance) maxDistance = distance; } cacheMaxDistance = maxDistance; dirtyMaxDistance = false; } -void AgentGroup::reportSizeDistribution ( const QVector& sizeDistributionIn ) +void AgentGroup::reportSizeDistribution(const QVector &sizeDistributionIn) { QString sizeDistributionString; int groupSize = 1; - foreach ( int count, sizeDistributionIn ) - { - sizeDistributionString += tr ( " %1: %2;" ).arg ( groupSize ).arg ( count ); + foreach (int count, sizeDistributionIn) { + sizeDistributionString += tr(" %1: %2;").arg(groupSize).arg(count); groupSize++; } - ROS_DEBUG("Group Size Distribution: %s", sizeDistributionString.toStdString().c_str()); + ROS_DEBUG("Group Size Distribution: %s", sizeDistributionString.toStdString().c_str()); } QString AgentGroup::toString() const { QString agentString; bool firstMember = true; - foreach ( Agent* agent, members ) - { - if ( !firstMember ) + foreach (Agent * agent, members) { + if (!firstMember) agentString += ", "; agentString += agent->toString(); firstMember = false; } - return tr ( "AgentGroup (CoM: @%1,%2; Members:%3)" ).arg ( cacheCoM.x ).arg ( cacheCoM.y ).arg ( agentString ); + return tr("AgentGroup (CoM: @%1,%2; Members:%3)").arg(cacheCoM.x).arg(cacheCoM.y).arg(agentString); } diff --git a/pedsim_simulator/src/simulator.cpp b/pedsim_simulator/src/simulator.cpp index 23d29c04..f185670b 100644 --- a/pedsim_simulator/src/simulator.cpp +++ b/pedsim_simulator/src/simulator.cpp @@ -35,8 +35,8 @@ #include -Simulator::Simulator ( const ros::NodeHandle &node ) - : nh_ ( node ) +Simulator::Simulator(const ros::NodeHandle &node) + : nh_(node) { // nothing to do here } @@ -46,6 +46,8 @@ Simulator::~Simulator() delete robot_; // shutdown service servers and publishers + // TODO - need a better way to keep track of publishers in a programmatic way and + // also to terminate them sub_robot_command_.shutdown(); pub_agent_visuals_.shutdown(); pub_agent_arrows_.shutdown(); @@ -68,43 +70,44 @@ bool Simulator::initializeSimulation() { /// setup ros publishers // visualizations - pub_agent_visuals_ = nh_.advertise ( "agents_markers", 0 ); - pub_agent_arrows_ = nh_.advertise ( "agent_arrows", 0 ); - pub_group_lines_ = nh_.advertise ( "group_lines", 0 ); - pub_walls_ = nh_.advertise ( "walls", 0 ); - pub_attractions_ = nh_.advertise ( "attractions", 0 ); - pub_queues_ = nh_.advertise ( "queues", 0 ); - pub_waypoints_ = nh_.advertise ( "waypoints", 0 ); + pub_agent_visuals_ = nh_.advertise ("agents_markers", 0); + pub_agent_arrows_ = nh_.advertise ("agent_arrows", 0); + pub_group_lines_ = nh_.advertise ("group_lines", 0); + pub_walls_ = nh_.advertise ("walls", 0); + pub_attractions_ = nh_.advertise ("attractions", 0); + pub_queues_ = nh_.advertise ("queues", 0); + pub_waypoints_ = nh_.advertise ("waypoints", 0); // informative topics (data) - pub_obstacles_ = nh_.advertise ( "static_obstacles", 0 ); - pub_all_agents_ = nh_.advertise ( "dynamic_obstacles", 0 ); + pub_obstacles_ = nh_.advertise ("static_obstacles", 0); + pub_all_agents_ = nh_.advertise ("dynamic_obstacles", 0); - pub_tracked_persons_ = nh_.advertise ( "/spencer/perception/tracked_persons", 0 ); - pub_tracked_groups_ = nh_.advertise ( "/spencer/perception/tracked_groups", 0 ); - pub_social_activities_ = nh_.advertise ( "/spencer/perception/social_activities", 0 ); + pub_tracked_persons_ = nh_.advertise ("/spencer/perception/tracked_persons", +0); + pub_tracked_groups_ = nh_.advertise ("/spencer/perception/tracked_groups", 0); + pub_social_activities_ = nh_.advertise +("/spencer/perception/social_activities", 0); /// setup any pointers - orientation_handler_.reset ( new OrientationHandler() ); + orientation_handler_.reset(new OrientationHandler()); robot_ = nullptr; /// subscribers - sub_robot_command_ = nh_.subscribe ( "/pedsim_simulator/robot_command", 1, &Simulator::callbackRobotCommand, this ); + sub_robot_command_ = nh_.subscribe("/pedsim_simulator/robot_command", 1, &Simulator::callbackRobotCommand, this); /// load parameters std::string scene_file_param; - ros::param::param ( "/simulator/scene_file", scene_file_param, "scene.xml" ); + ros::param::param ("/simulator/scene_file", scene_file_param, "scene.xml"); // load scenario file - QString scenefile = QString::fromStdString ( scene_file_param ); + QString scenefile = QString::fromStdString(scene_file_param); ScenarioReader scenario_reader; - bool readResult = scenario_reader.readFromFile ( scenefile ); - if ( readResult == false ) - { - ROS_WARN ( "Could not load the scene file, check paths" ); + bool readResult = scenario_reader.readFromFile(scenefile); + if (readResult == false) { + ROS_WARN("Could not load the scene file, check paths"); return false; } - /// load the remaining parameters + /// load the remaining parameters loadConfigParameters(); return true; @@ -117,17 +120,17 @@ bool Simulator::initializeSimulation() /// ----------------------------------------------------------------- void Simulator::loadConfigParameters() { - double robot_wait_time; - ros::param::param ( "/pedsim_simulator/robot_wait_time", robot_wait_time, 10.0 ); - CONFIG.robot_wait_time = robot_wait_time; - - double max_robot_speed; - ros::param::param ( "/pedsim_simulator/max_robot_speed", max_robot_speed, 2.0 ); - CONFIG.max_robot_speed = max_robot_speed; - + double robot_wait_time; + ros::param::param ("/pedsim_simulator/robot_wait_time", robot_wait_time, 10.0); + CONFIG.robot_wait_time = robot_wait_time; + + double max_robot_speed; + ros::param::param ("/pedsim_simulator/max_robot_speed", max_robot_speed, 2.0); + CONFIG.max_robot_speed = max_robot_speed; + double teleop_flag; - ros::param::param ( "/pedsim_simulator/teleop_flag", teleop_flag, 0.0 ); - CONFIG.robot_mode = static_cast ( teleop_flag ); + ros::param::param ("/pedsim_simulator/teleop_flag", teleop_flag, 0.0); + CONFIG.robot_mode = static_cast(teleop_flag); } @@ -137,19 +140,16 @@ void Simulator::loadConfigParameters() /// ----------------------------------------------------------------- void Simulator::runSimulation() { - ros::Rate r ( 25 ); // Hz - - while ( ros::ok() ) - { - if ( SCENE.getTime() < 0.1 ) - { - // setup the robot - BOOST_FOREACH ( Agent* a, SCENE.getAgents() ) - { - if ( a->getType() == Ped::Tagent::ROBOT ) - robot_ = a; - } - } + ros::Rate r(25); // Hz + + while (ros::ok()) { + if (SCENE.getTime() < 0.1) { + // setup the robot + BOOST_FOREACH(Agent * a, SCENE.getAgents()) { + if (a->getType() == Ped::Tagent::ROBOT) + robot_ = a; + } + } SCENE.moveAllAgents(); @@ -162,8 +162,7 @@ void Simulator::runSimulation() publishObstacles(); // only publish the obstacles in the beginning - if ( SCENE.getTime() < 10 ) - { + if (SCENE.getTime() < 10) { publishAttractions(); publishWalls(); } @@ -180,21 +179,19 @@ void Simulator::runSimulation() /// Listens to incoming messages and manipulates the robot. /// \param[in] msg Message containing the pos and vel for the robot /// ----------------------------------------------------------------- -void Simulator::callbackRobotCommand ( const pedsim_msgs::AgentState::ConstPtr &msg ) +void Simulator::callbackRobotCommand(const pedsim_msgs::AgentState::ConstPtr &msg) { double vx = msg->velocity.x; double vy = msg->velocity.y; - if ( CONFIG.robot_mode == RobotMode::TELEOPERATION || CONFIG.robot_mode == RobotMode::CONTROLLED ) - robot_->setTeleop ( true ); + if (CONFIG.robot_mode == RobotMode::TELEOPERATION || CONFIG.robot_mode == RobotMode::CONTROLLED) + robot_->setTeleop(true); - if ( robot_->getType() == static_cast ( msg->type ) ) - { - robot_->setvx ( vx ); - robot_->setvy ( vy ); + if (robot_->getType() == static_cast(msg->type)) { + robot_->setvx(vx); + robot_->setvy(vy); // NOTE - check if this is really necessary -// robot_->setVmax ( sqrt ( vx * vx + vy * vy ) ); - robot_->setVmax( CONFIG.max_robot_speed ); + robot_->setVmax(CONFIG.max_robot_speed); } } @@ -217,56 +214,50 @@ void Simulator::publishSocialActivities() spencer_social_relation_msgs::SocialActivity group_moving_activity; spencer_social_relation_msgs::SocialActivity individual_moving_activity; - BOOST_FOREACH ( Agent* a, SCENE.getAgents() ) - { + foreach(Agent * a, SCENE.getAgents()) { /// activity of the current agent AgentStateMachine::AgentState sact = a->getStateMachine()->getCurrentState(); - if ( sact == AgentStateMachine::AgentState::StateQueueing ) - { + if (sact == AgentStateMachine::AgentState::StateQueueing) { queueing_activity.type = spencer_social_relation_msgs::SocialActivity::TYPE_WAITING_IN_QUEUE; queueing_activity.confidence = 1.0; - queueing_activity.track_ids.push_back( a->getId() ); + queueing_activity.track_ids.push_back(a->getId()); } - if ( sact == AgentStateMachine::AgentState::StateShopping ) - { + if (sact == AgentStateMachine::AgentState::StateShopping) { shopping_activity.type = spencer_social_relation_msgs::SocialActivity::TYPE_SHOPPING; shopping_activity.confidence = 1.0; - shopping_activity.track_ids.push_back( a->getId() ); + shopping_activity.track_ids.push_back(a->getId()); } - if ( a->getType() == Ped::Tagent::ELDER ) // Hack for really slow people - { + if (a->getType() == Ped::Tagent::ELDER) { // Hack for really slow people standing_activity.type = spencer_social_relation_msgs::SocialActivity::TYPE_STANDING; standing_activity.confidence = 1.0; - standing_activity.track_ids.push_back( a->getId() ); + standing_activity.track_ids.push_back(a->getId()); } - if ( sact == AgentStateMachine::AgentState::StateGroupWalking ) - { + if (sact == AgentStateMachine::AgentState::StateGroupWalking) { group_moving_activity.type = spencer_social_relation_msgs::SocialActivity::TYPE_GROUP_MOVING; group_moving_activity.confidence = 1.0; - group_moving_activity.track_ids.push_back( a->getId() ); + group_moving_activity.track_ids.push_back(a->getId()); } - if ( sact == AgentStateMachine::AgentState::StateWalking ) - { + if (sact == AgentStateMachine::AgentState::StateWalking) { individual_moving_activity.type = spencer_social_relation_msgs::SocialActivity::TYPE_INDIVIDUAL_MOVING; individual_moving_activity.confidence = 1.0; - individual_moving_activity.track_ids.push_back( a->getId() ); + individual_moving_activity.track_ids.push_back(a->getId()); } // else // continue; } - social_activities.elements.push_back( queueing_activity ); - social_activities.elements.push_back( shopping_activity ); - social_activities.elements.push_back( standing_activity ); - social_activities.elements.push_back( group_moving_activity ); - social_activities.elements.push_back( individual_moving_activity ); + social_activities.elements.push_back(queueing_activity); + social_activities.elements.push_back(shopping_activity); + social_activities.elements.push_back(standing_activity); + social_activities.elements.push_back(group_moving_activity); + social_activities.elements.push_back(individual_moving_activity); - pub_social_activities_.publish( social_activities ); + pub_social_activities_.publish(social_activities); } @@ -282,16 +273,15 @@ void Simulator::publishData() tracked_people_header.stamp = ros::Time::now(); tracked_people.header = tracked_people_header; - BOOST_FOREACH ( Agent* a, SCENE.getAgents() ) - { + foreach(Agent * a, SCENE.getAgents()) { spencer_tracking_msgs::TrackedPerson person; person.track_id = a->getId(); person.is_occluded = false; // person.detection_id = 0; // not simulated yet // person.age = 0; // also not simulated yet - double theta = atan2 ( a->getvy(), a->getvx() ); - Eigen::Quaternionf q = orientation_handler_->angle2Quaternion ( theta ); + double theta = atan2(a->getvy(), a->getvx()); + Eigen::Quaternionf q = orientation_handler_->angle2Quaternion(theta); geometry_msgs::PoseWithCovariance pcov; pcov.pose.position.x = a->getx(); @@ -313,7 +303,7 @@ void Simulator::publishData() // tcov.twist.angular.z = 0; person.twist = tcov; - tracked_people.tracks.push_back( person ); + tracked_people.tracks.push_back(person); } /// Tracked groups @@ -322,17 +312,15 @@ void Simulator::publishData() tracked_groups_header.stamp = ros::Time::now(); tracked_groups.header = tracked_groups_header; - QList sim_groups = SCENE.getGroups(); - BOOST_FOREACH ( AgentGroup* ag, sim_groups ) - { + QList sim_groups = SCENE.getGroups(); + foreach(AgentGroup * ag, sim_groups) { spencer_tracking_msgs::TrackedGroup group; group.group_id = ag->getId(); // group.age = 0; //NOTE not simulated so far Ped::Tvector com = ag->getCenterOfMass(); // group.centerOfGravity = ... // TODO - convert CoM to Pose with Covariance - BOOST_FOREACH ( Agent* m, ag->getMembers() ) - { + BOOST_FOREACH(Agent * m, ag->getMembers()) { group.track_ids.push_back(m->getId()); } @@ -340,8 +328,8 @@ void Simulator::publishData() } /// publish the messages - pub_tracked_persons_.publish( tracked_people ); - pub_tracked_groups_.publish( tracked_groups ); + pub_tracked_persons_.publish(tracked_people); + pub_tracked_groups_.publish(tracked_groups); } @@ -361,8 +349,7 @@ void Simulator::publishAgents() all_header.stamp = ros::Time::now(); all_status.header = all_header; - BOOST_FOREACH ( Agent* a, SCENE.getAgents() ) - { + foreach(Agent * a, SCENE.getAgents()) { /// walking people message animated_marker_msgs::AnimatedMarker marker; marker.mesh_use_embedded_materials = true; @@ -375,9 +362,11 @@ void Simulator::publishAgents() marker.pose.position.x = a->getx(); marker.pose.position.y = a->gety(); - marker.action = 0; // add or modify + marker.action = 0; // add or modify const double person_scale = 2.0 / 8.5 * 1.8; // TODO - move these magic numbers to a config file - marker.scale.x = person_scale; marker.scale.y = person_scale; marker.scale.z = person_scale; + marker.scale.x = person_scale; + marker.scale.y = person_scale; + marker.scale.z = person_scale; /// arrows visualization_msgs::Marker arrow; @@ -390,61 +379,75 @@ void Simulator::publishAgents() arrow.pose.position.y = a->gety(); arrow.pose.position.z = 1.4; arrow.action = 0; // add or modify - arrow.color.a = 1.0; arrow.color.r = 1.0; arrow.color.g = 0.0; arrow.color.b = 0.0; - arrow.scale.y = 0.05; arrow.scale.z = 0.05; - - if ( a->getType() == Ped::Tagent::ELDER ) - { - marker.color.a = 1.0; marker.color.r = 1.0; marker.color.g = 1.0; marker.color.b = 1.0; + arrow.color.a = 1.0; + arrow.color.r = 1.0; + arrow.color.g = 0.0; + arrow.color.b = 0.0; + arrow.scale.y = 0.05; + arrow.scale.z = 0.05; + + if (a->getType() == Ped::Tagent::ELDER) { + marker.color.a = 1.0; + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 1.0; + } else { + marker.color.a = 1.0; + marker.color.r = 0.0; + marker.color.g = 0.7; + marker.color.b = 1.0; } - else - { - marker.color.a = 1.0; marker.color.r = 0.0; marker.color.g = 0.7; marker.color.b = 1.0; - } - - if ( a->getStateMachine()->getCurrentState() == AgentStateMachine::AgentState::StateQueueing ) - { - marker.color.a = 1.0; marker.color.r = 1.0; marker.color.g = 0.0; marker.color.b = 1.0; + + if (a->getStateMachine()->getCurrentState() == AgentStateMachine::AgentState::StateQueueing) { + marker.color.a = 1.0; + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 1.0; } - if ( a->getStateMachine()->getCurrentState() == AgentStateMachine::AgentState::StateShopping ) - { - marker.color.a = 1.0; marker.color.r = 0.0; marker.color.g = 0.0; marker.color.b = 1.0; + if (a->getStateMachine()->getCurrentState() == AgentStateMachine::AgentState::StateShopping) { + marker.color.a = 1.0; + marker.color.r = 0.0; + marker.color.g = 0.0; + marker.color.b = 1.0; } - Eigen::Quaternionf q = computePose( a ); + Eigen::Quaternionf q = computePose(a); marker.pose.orientation.x = q.x(); marker.pose.orientation.y = q.y(); marker.pose.orientation.z = q.z(); marker.pose.orientation.w = q.w(); - double theta = atan2 ( a->getvy(), a->getvx() ); - Eigen::Quaternionf qa = orientation_handler_->angle2Quaternion ( theta ); + double theta = atan2(a->getvy(), a->getvx()); + Eigen::Quaternionf qa = orientation_handler_->angle2Quaternion(theta); - if ( a->getvx() != 0.0 ) - { + if (a->getvx() != 0.0) { arrow.pose.orientation.x = qa.x(); arrow.pose.orientation.y = qa.y(); arrow.pose.orientation.z = qa.z(); arrow.pose.orientation.w = qa.w(); - double xx = sqrt(a->getvx()*a->getvx() + a->getvy()*a->getvy()); + double xx = sqrt(a->getvx() * a->getvx() + a->getvy() * a->getvy()); arrow.scale.x = xx > 0.0 ? xx : 0.01; marker.animation_speed = xx * 0.7; - } - else - { + } else { marker.animation_speed = 0.0; } - if ( robot_ != nullptr && a->getType() == robot_->getType() ) - { + if (robot_ != nullptr && a->getType() == robot_->getType()) { marker.type = visualization_msgs::Marker::MESH_RESOURCE; + // TODO - this should be a configurable parameter via launch file marker.mesh_resource = "package://pedsim_simulator/images/darylbot_rotated_shifted.dae"; - marker.color.a = 1.0; marker.color.r = 1.0; marker.color.g = 1.0; marker.color.b = 1.0; - marker.scale.x = 0.8; marker.scale.y = 0.8; marker.scale.z = 1.0; + marker.color.a = 1.0; + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 1.0; + + marker.scale.x = 0.8; + marker.scale.y = 0.8; + marker.scale.z = 1.0; marker.pose.orientation.x = qa.x(); marker.pose.orientation.y = qa.y(); @@ -455,8 +458,8 @@ void Simulator::publishAgents() arrow.pose.position.z = 1.0; } - marker_array.markers.push_back ( marker ); - arrow_array.markers.push_back ( arrow ); + marker_array.markers.push_back(marker); + arrow_array.markers.push_back(arrow); /// status message /// TODO - remove this once, we publish internal states using @@ -477,18 +480,18 @@ void Simulator::publishAgents() state.velocity.z = a->getvz(); AgentStateMachine::AgentState sc = a->getStateMachine()->getCurrentState(); - state.social_state = agentStateToActivity ( sc ); - if ( a->getType() == Ped::Tagent::ELDER ) + state.social_state = agentStateToActivity(sc); + if (a->getType() == Ped::Tagent::ELDER) state.social_state = pedsim_msgs::AgentState::TYPE_STANDING; - all_status.agent_states.push_back ( state ); + all_status.agent_states.push_back(state); } // publish the marker array - pub_agent_visuals_.publish ( marker_array ); - pub_agent_arrows_.publish ( arrow_array ); + pub_agent_visuals_.publish(marker_array); + pub_agent_arrows_.publish(arrow_array); - pub_all_agents_.publish ( all_status ); + pub_all_agents_.publish(all_status); } @@ -498,41 +501,48 @@ void Simulator::publishAgents() /// ----------------------------------------------------------------- void Simulator::publishGroupVisuals() { - QList groups = SCENE.getGroups(); + QList groups = SCENE.getGroups(); /// visualize groups (sketchy) - BOOST_FOREACH ( AgentGroup* ag, groups ) - { + foreach(AgentGroup * ag, groups) { // skip empty ones - if ( ag->memberCount() < 1 ) + if (ag->memberCount() < 1) continue; /// members of the group geometry_msgs::Point p1; Ped::Tvector gcom = ag->getCenterOfMass(); - p1.x = gcom.x; p1.y = gcom.y; p1.z = 1.4; + p1.x = gcom.x; + p1.y = gcom.y; + p1.z = 1.4; visualization_msgs::MarkerArray lines_array; - BOOST_FOREACH ( Agent* m, ag->getMembers() ) - { + foreach(Agent * m, ag->getMembers()) { visualization_msgs::Marker marker; marker.header.frame_id = "world"; marker.header.stamp = ros::Time(); marker.ns = "pedsim"; - marker.id = m->getId() +1000; - - marker.color.a = 0.7; marker.color.r = 1.0; marker.color.g = 1.0; marker.color.b = 0.0; - marker.scale.x = 0.05; marker.scale.y = 0.05; marker.scale.z = 0.05; + marker.id = m->getId() + 1000; + + marker.color.a = 0.7; + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + marker.scale.x = 0.05; + marker.scale.y = 0.05; + marker.scale.z = 0.05; marker.type = visualization_msgs::Marker::ARROW; geometry_msgs::Point p2; - p2.x = m->getx(); p2.y = m->gety(); p2.z = 1.4; + p2.x = m->getx(); + p2.y = m->gety(); + p2.z = 1.4; - marker.points.push_back ( p1 ); - marker.points.push_back ( p2 ); - lines_array.markers.push_back ( marker ); + marker.points.push_back(p1); + marker.points.push_back(p2); + lines_array.markers.push_back(marker); } - pub_group_lines_.publish ( lines_array ); + pub_group_lines_.publish(lines_array); } } @@ -550,16 +560,17 @@ void Simulator::publishObstacles() obstacles.cell_height = 1.0; std::vector::const_iterator it = SCENE.obstacle_cells_.begin(); - while ( it != SCENE.obstacle_cells_.end() ) - { + while (it != SCENE.obstacle_cells_.end()) { geometry_msgs::Point p; - Location loc = ( *it ); - p.x = loc.x - 0.5; p.y = loc.y - 0.5; p.z = 0.0; - obstacles.cells.push_back ( p ); + Location loc = (*it); + p.x = loc.x - 0.5; + p.y = loc.y - 0.5; + p.z = 0.0; + obstacles.cells.push_back(p); it++; } - pub_obstacles_.publish ( obstacles ); + pub_obstacles_.publish(obstacles); } @@ -575,24 +586,28 @@ void Simulator::publishWalls() marker.header.stamp = ros::Time(); marker.ns = "pedsim"; marker.id = 10000; - marker.color.a = 1.0; marker.color.r = 1.0; marker.color.g = 0.0; marker.color.b = 0.0; - marker.scale.x = 1.0; marker.scale.y = 1.0; marker.scale.z = 3.0; + marker.color.a = 1.0; + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + marker.scale.x = 1.0; + marker.scale.y = 1.0; + marker.scale.z = 3.0; marker.pose.position.z = marker.scale.z / 2.0; marker.type = visualization_msgs::Marker::CUBE_LIST; std::vector::const_iterator it = SCENE.obstacle_cells_.begin(); - while ( it != SCENE.obstacle_cells_.end() ) - { + while (it != SCENE.obstacle_cells_.end()) { geometry_msgs::Point p; - Location loc = ( *it ); + Location loc = (*it); p.x = loc.x; p.y = loc.y; p.z = 0.0; - marker.points.push_back ( p ); + marker.points.push_back(p); it++; } - pub_walls_.publish ( marker ); + pub_walls_.publish(marker); } @@ -603,7 +618,7 @@ void Simulator::publishWalls() /// ----------------------------------------------------------------- void Simulator::publishAttractions() { - /// waypoints + /// waypoints // BOOST_FOREACH ( Waypoint* wp, SCENE.getWaypoints() ) // { // // wp->getType() @@ -632,49 +647,48 @@ void Simulator::publishAttractions() // pub_waypoints_.publish ( marker ); // } - // WaitingQueue* info_desk = SCENE.getWaitingQueueByName("klm"); - // visualization_msgs::Marker marker; - // marker.header.frame_id = "world"; - // marker.header.stamp = ros::Time(); - // marker.ns = "pedsim"; - // marker.id = info_desk->getId(); - // marker.type = visualization_msgs::Marker::MESH_RESOURCE; - // marker.mesh_resource = "package://pedsim_simulator/images/kiosk.dae"; +// WaitingQueue* info_desk = SCENE.getWaitingQueueByName("klm"); + // visualization_msgs::Marker marker; + // marker.header.frame_id = "world"; + // marker.header.stamp = ros::Time(); + // marker.ns = "pedsim"; + // marker.id = info_desk->getId(); + // marker.type = visualization_msgs::Marker::MESH_RESOURCE; +// marker.mesh_resource = "package://pedsim_simulator/images/kiosk.dae"; - // marker.scale.x = 1.0; - // marker.scale.y = 1.0; - // marker.scale.z = 1.0; + // marker.scale.x = 1.0; + // marker.scale.y = 1.0; + // marker.scale.z = 1.0; - // marker.pose.position.x = info_desk->getPosition().x - 2.5; - // marker.pose.position.y = info_desk->getPosition().y + 0.5; - // marker.pose.position.z = 0.0; //marker.scale.z / 2.0; + // marker.pose.position.x = info_desk->getPosition().x - 2.5; + // marker.pose.position.y = info_desk->getPosition().y + 0.5; + // marker.pose.position.z = 0.0; //marker.scale.z / 2.0; - // pub_queues_.publish ( marker ); + // pub_queues_.publish ( marker ); - // WaitingQueue* kq = SCENE.getWaitingQueueByName("kq"); - // visualization_msgs::Marker marker2; - // marker2.header.frame_id = "world"; - // marker2.header.stamp = ros::Time(); - // marker2.ns = "pedsim"; - // marker2.id = kq->getId(); - // marker2.type = visualization_msgs::Marker::MESH_RESOURCE; - // marker2.mesh_resource = "package://pedsim_simulator/images/kiosk.dae"; + // WaitingQueue* kq = SCENE.getWaitingQueueByName("kq"); + // visualization_msgs::Marker marker2; + // marker2.header.frame_id = "world"; + // marker2.header.stamp = ros::Time(); + // marker2.ns = "pedsim"; + // marker2.id = kq->getId(); + // marker2.type = visualization_msgs::Marker::MESH_RESOURCE; +// marker2.mesh_resource = "package://pedsim_simulator/images/kiosk.dae"; - // marker2.scale.x = 1.0; - // marker2.scale.y = 1.0; - // marker2.scale.z = 1.0; + // marker2.scale.x = 1.0; + // marker2.scale.y = 1.0; + // marker2.scale.z = 1.0; - // marker2.pose.position.x = kq->getPosition().x - 2.5; - // marker2.pose.position.y = kq->getPosition().y + 0.5; - // marker2.pose.position.z = 0.0; //marker.scale.z / 2.0; + // marker2.pose.position.x = kq->getPosition().x - 2.5; + // marker2.pose.position.y = kq->getPosition().y + 0.5; + // marker2.pose.position.z = 0.0; //marker.scale.z / 2.0; - // pub_queues_.publish ( marker2 ); + // pub_queues_.publish ( marker2 ); /// publish attractions (shopping areas etc) - BOOST_FOREACH ( AttractionArea* atr, SCENE.getAttractions() ) - { + foreach(AttractionArea * atr, SCENE.getAttractions()) { visualization_msgs::Marker marker; marker.header.frame_id = "world"; marker.header.stamp = ros::Time(); @@ -696,7 +710,7 @@ void Simulator::publishAttractions() marker.type = visualization_msgs::Marker::CUBE; - pub_attractions_.publish ( marker ); + pub_attractions_.publish(marker); } } @@ -705,23 +719,20 @@ void Simulator::publishAttractions() /// \brief main /// Hub of the application /// ----------------------------------------------------------------- -int main ( int argc, char **argv ) +int main(int argc, char **argv) { - QApplication app ( argc, argv ); + QApplication app(argc, argv); // initialize resources - ros::init ( argc, argv, "simulator" ); + ros::init(argc, argv, "simulator"); ros::NodeHandle node; - Simulator sm ( node ); + Simulator sm(node); - if ( sm.initializeSimulation() ) - { - ROS_INFO ( "node initialized, now running " ); + if (sm.initializeSimulation()) { + ROS_INFO("node initialized, now running "); sm.runSimulation(); - } - else - { + } else { ROS_WARN("Could not initialize simulation, aborting"); return EXIT_FAILURE; } diff --git a/pedsim_simulator/src/waypointplanner/individualwaypointplanner.cpp b/pedsim_simulator/src/waypointplanner/individualwaypointplanner.cpp index b506318e..0503371c 100644 --- a/pedsim_simulator/src/waypointplanner/individualwaypointplanner.cpp +++ b/pedsim_simulator/src/waypointplanner/individualwaypointplanner.cpp @@ -45,17 +45,6 @@ IndividualWaypointPlanner::IndividualWaypointPlanner() bool IndividualWaypointPlanner::setAgent ( Agent* agentIn ) { agent = agentIn; - - if (agent->getType() == Ped::Tagent::ELDER) - { - /// discourage fancy behaviour on elderly people -// agent->disableForce ( "Social" ); -// agent->disableForce ( "Random" ); -// agent->disableForce ( "GroupCoherence" ); -// agent->disableForce ( "GroupGaze" ); -// agent->disableForce ( "GroupRepulsion" ); - } - return true; } diff --git a/pedsim_simulator/src/waypointplanner/queueingplanner.cpp b/pedsim_simulator/src/waypointplanner/queueingplanner.cpp index b098b12c..61675d16 100644 --- a/pedsim_simulator/src/waypointplanner/queueingplanner.cpp +++ b/pedsim_simulator/src/waypointplanner/queueingplanner.cpp @@ -34,7 +34,6 @@ #include #include #include - #include @@ -206,7 +205,6 @@ WaitingQueue* QueueingWaypointPlanner::getWaitingQueue() const bool QueueingWaypointPlanner::hasReachedQueueEnd() const { const double endPositionRadius = 2.0; -// const double endPositionRadius = 5.0; // sanity checks if ( waitingQueue == nullptr ) @@ -275,6 +273,7 @@ void QueueingWaypointPlanner::activateQueueingMode() currentWaypoint = new QueueingWaypoint ( waypointName, queueingPosition ); } +/// Affects the behavior at the end of the queue and hence the shape void QueueingWaypointPlanner::addPrivateSpace ( Ped::Tvector& queueEndIn ) const { // const double privateSpaceDistance = 0.4;