Advertisement
130s

Code_Q2_Server_11Oct20

Oct 20th, 2011
2,300
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include "ros/ros.h"
  2. #include "geometry_msgs/Point.h"
  3.  
  4. #include "server_msgs/Swarm.h"
  5.  
  6. using namespace Crowd;
  7.  
  8. static CCrowd g_crowd(20, 20, 20, 20, 0, 0); //width, height, completeWidth, completeHeight,widthOffset, heightOffset
  9. static int g_globalDiscomfort = -1;
  10.  
  11. class CdAtor {
  12. public:
  13. CdAtor() {
  14. std::string topicName = "server_msgs/Swarm";
  15. publisher = n.advertise<geometry_msgs::Point>(topicName, 1000);
  16. }
  17. void run(int argc, char* argv[]) {
  18. // Some logic.
  19.  
  20. ros::Rate r(5); //5 Hz
  21. while (ros::ok) {
  22. display();
  23. r.sleep();
  24. }
  25. ros::shutdown();
  26. }
  27.  
  28. void display() {
  29. usleep(10000);
  30. crowdAllocator::Swarm rSwarm;
  31. for (int swarmCount = 0; swarmCount < g_crowd.SwarmCount();
  32. ++swarmCount) {
  33. for (int goalCount = 0; goalCount < swarm.GoalCount();
  34. ++goalCount) {
  35.  
  36. // some logic
  37. for (int personCount = 0; personCount < swarm.PersonCount();++personCount) {
  38. geometry_msgs::Point rPerson; // rPerson = Person instance in ROS format.
  39. rPerson.x = person.position.x;
  40. rPerson.y = person.position.y;
  41. rPerson.z = 0;
  42. rSwarm.crowds.push_back(rPerson);
  43. }
  44. }
  45. }
  46. publisher.publish(rSwarm);
  47. }
  48.  
  49. private:
  50. ros::NodeHandle n;
  51. ros::Publisher publisher;
  52. };
  53.  
  54. main(int argc, char* argv[]){
  55. std::string nodeName = "CdAtor";
  56. ros::init(argc, argv, nodeName);
  57. CdAtor crowdAlloc = CdAtor();
  58. crowdAlloc.run(argc, argv);
  59. return 0;
  60. }
  61.  
  62.  
Advertisement
RAW Paste Data Copied
Advertisement