Flatland
bumper.h
1 /*
2  * ______ __ __ __
3  * /\ _ \ __ /\ \/\ \ /\ \__
4  * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____
5  * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\
6  * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\
7  * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/
8  * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/
9  * @copyright Copyright 2017 Avidbots Corp.
10  * @name bumper.h
11  * @brief Bumper plugin
12  * @author Chunshang Li
13  *
14  * Software License Agreement (BSD License)
15  *
16  * Copyright (c) 2017, Avidbots Corp.
17  * All rights reserved.
18  *
19  * Redistribution and use in source and binary forms, with or without
20  * modification, are permitted provided that the following conditions
21  * are met:
22  *
23  * * Redistributions of source code must retain the above copyright
24  * notice, this list of conditions and the following disclaimer.
25  * * Redistributions in binary form must reproduce the above
26  * copyright notice, this list of conditions and the following
27  * disclaimer in the documentation and/or other materials provided
28  * with the distribution.
29  * * Neither the name of the Avidbots Corp. nor the names of its
30  * contributors may be used to endorse or promote products derived
31  * from this software without specific prior written permission.
32  *
33  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
34  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
35  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
36  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
37  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
38  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
39  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
40  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
41  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
42  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
43  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
44  * POSSIBILITY OF SUCH DAMAGE.
45  */
46 
47 #include <flatland_plugins/update_timer.h>
48 #include <flatland_server/model_plugin.h>
49 #include <ros/ros.h>
50 
51 #ifndef FLATLAND_PLUGINS_BUMPER_H
52 #define FLATLAND_PLUGINS_BUMPER_H
53 
54 using namespace flatland_server;
55 
56 namespace flatland_plugins {
57 
62 class Bumper : public ModelPlugin {
63  public:
64  struct ContactState {
65  int num_count;
66  double sum_normal_impulses[2];
67  double sum_tangential_impulses[2];
68  b2Vec2 points[2];
69  b2Vec2 normal;
70  int normal_sign;
71 
72  Body *body_A;
73  Body *body_B;
74  Entity *entity_B;
75 
76  ContactState();
77  void Reset();
78  };
79 
80  std::string topic_name_;
81  std::string world_frame_id_;
82  std::vector<Body *> excluded_bodies_;
83  bool publish_all_collisions_;
85  double update_rate_;
86 
87  UpdateTimer update_timer_;
88 
90  std::map<b2Contact *, ContactState> contact_states_;
91  ros::Publisher collisions_publisher_;
92 
97  void OnInitialize(const YAML::Node &config) override;
98 
103  void BeforePhysicsStep(const Timekeeper &timekeeper) override;
104 
109  void AfterPhysicsStep(const Timekeeper &timekeeper) override;
110 
115  void BeginContact(b2Contact *contact) override;
116 
121  void EndContact(b2Contact *contact) override;
122 
123  /*
124  * @brief A method that is called for Box2D presolve
125  * @param[in] contact Box2D contact
126  * @param[in] oldManifold Manifold from the previous iteration
127  */
128  void PostSolve(b2Contact *contact, const b2ContactImpulse *impulse) override;
129 };
130 };
131 
132 #endif
flatland_plugins::Bumper::ContactState::normal
b2Vec2 normal
normal of collision points, all points have same normal
Definition: bumper.h:69
flatland_plugins::Bumper::excluded_bodies_
std::vector< Body * > excluded_bodies_
Definition: bumper.h:82
flatland_plugins::Bumper::ContactState::num_count
int num_count
stores number of times post solve is called
Definition: bumper.h:65
flatland_plugins
Definition: dummy_model_plugin.h:59
flatland_server::Entity
Definition: entity.h:61
flatland_server
Definition: body.h:55
flatland_plugins::Bumper::update_timer_
UpdateTimer update_timer_
for managing update rate
Definition: bumper.h:87
flatland_plugins::Bumper::ContactState
Definition: bumper.h:64
flatland_plugins::Bumper::ContactState::body_A
Body * body_A
the body of the model involved in the collision
Definition: bumper.h:72
flatland_plugins::Bumper::world_frame_id_
std::string world_frame_id_
name of the world frame id
Definition: bumper.h:81
flatland_plugins::Bumper::collisions_publisher_
ros::Publisher collisions_publisher_
For publishing the collisions.
Definition: bumper.h:91
flatland_plugins::Bumper::update_rate_
double update_rate_
rate to publish message at
Definition: bumper.h:85
flatland_server::Timekeeper
Definition: timekeeper.h:55
flatland_plugins::Bumper::contact_states_
std::map< b2Contact *, ContactState > contact_states_
For keeping track of contacts.
Definition: bumper.h:90
flatland_server::ModelPlugin
Definition: model_plugin.h:64
flatland_plugins::Bumper
Definition: bumper.h:62
flatland_server::Body
Definition: body.h:61
flatland_plugins::Bumper::ContactState::normal_sign
int normal_sign
for flipping direction of normal when necessary
Definition: bumper.h:70
flatland_plugins::Bumper::ContactState::body_B
Body * body_B
the other body involved in the collision
Definition: bumper.h:73
flatland_plugins::UpdateTimer
Definition: update_timer.h:55
Generated on Mon Jun 4 2018 19:08:05 for Flatland by   doxygen 1.8.11