Advertisement
Guest User

Untitled

a guest
Aug 27th, 2015
473
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 2.12 KB | None | 0 0
  1. #include <gazebo/transport/transport.hh>
  2. #include <gazebo/msgs/msgs.hh>
  3. #include <gazebo/gazebo.hh>
  4. #include <nav_msgs/Odometry.h>
  5. #include <ros/ros.h>
  6. #include <geometry_msgs/Vector3.h>
  7.  
  8. #include <iostream>
  9.  
  10. ros::Publisher pub;
  11. bool airborne;
  12.  
  13. // Forces callback function
  14. void forcesCb(ConstContactsPtr &_msg){
  15. geometry_msgs::Vector3 msgForce;
  16. // What to do when callback
  17. if ( airborne == false ) {
  18. //std::cout << "Collision but not airborn" << '\n';
  19. msgForce.x = 0;
  20. msgForce.y = 0;
  21. msgForce.z = 0;
  22. } else if ( _msg->contact_size() !=0 ) {
  23. //std::cout << "Collision" << '\n';
  24. msgForce.x = _msg->contact(0).wrench().Get(0).body_1_wrench().force().x();
  25. msgForce.y = _msg->contact(0).wrench().Get(0).body_1_wrench().force().y();
  26. msgForce.z = _msg->contact(0).wrench().Get(0).body_1_wrench().force().z();
  27. } else {
  28. //std::cout << "No collision" << '\n';
  29. msgForce.x = 0;
  30. msgForce.y = 0;
  31. msgForce.z = 0;
  32. }
  33. pub.publish(msgForce);
  34. }
  35.  
  36. // Position callback function
  37. void positionCb(const nav_msgs::Odometry::ConstPtr& msg2){
  38. if (msg2->pose.pose.position.z > 0.3) {
  39. airborne = true;
  40. } else {
  41. airborne = false;
  42. }
  43. }
  44.  
  45. int main(int _argc, char **_argv){
  46. // Set variables
  47. airborne = false;
  48.  
  49. // Load Gazebo & ROS
  50. gazebo::setupClient(_argc, _argv);
  51. ros::init(_argc, _argv, "talker");
  52.  
  53. // Create Gazebo node and init
  54. gazebo::transport::NodePtr node(new gazebo::transport::Node());
  55. node->Init();
  56.  
  57. // Create ROS node and init
  58. ros::NodeHandle n;
  59. pub = n.advertise<geometry_msgs::Vector3>("forces", 1000);
  60.  
  61. // Listen to Gazebo contacts topic
  62. gazebo::transport::SubscriberPtr sub = node->Subscribe("~/quadrotor/base_link/quadrotor_bumper/contacts", forcesCb);
  63.  
  64. // Listen to ROS for position
  65. ros::Subscriber sub2 = n.subscribe("/ground_truth/state", 1000, positionCb);
  66.  
  67. // Busy wait loop
  68. while (true) {
  69. gazebo::common::Time::MSleep(20);
  70. ros::spinOnce();
  71. }
  72.  
  73. // Mayke sure to shut everything down.
  74. gazebo::shutdown();
  75. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement