Guest User

Untitled

a guest
Aug 27th, 2015
185
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  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. }
RAW Paste Data

Adblocker detected! Please consider disabling it...

We've detected AdBlock Plus or some other adblocking software preventing Pastebin.com from fully loading.

We don't have any obnoxious sound, or popup ads, we actively block these annoying types of ads!

Please add Pastebin.com to your ad blocker whitelist or disable your adblocking software.

×