Skip to content

Commit

Permalink
minor tweaks
Browse files Browse the repository at this point in the history
  • Loading branch information
@makokal
makokal committed Aug 22, 2014
1 parent 144d4cd commit 1c3da08
Show file tree
Hide file tree
Showing 6 changed files with 396 additions and 425 deletions.
10 changes: 3 additions & 7 deletions pedsim_simulator/include/pedsim_simulator/agentstatemachine.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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();
Expand All @@ -84,7 +81,6 @@ public slots:
bool checkGroupForAttractions(AttractionArea** attractionOut = nullptr) const;
QString stateToName(AgentState stateIn) const;


// Attributes
protected:
Agent* agent;
Expand Down
152 changes: 75 additions & 77 deletions pedsim_simulator/include/pedsim_simulator/simulator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,94 +80,92 @@
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
OrientationHandlerPtr orientation_handler_;

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;
}
};
Loading

0 comments on commit 1c3da08

Please sign in to comment.