Gazebo Gazebo

API Reference

6.16.0
Visualization.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2020 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16  */
17 
18 #ifndef IGNITION_GAZEBO_SYSTEMS_OPTICAL_TACTILE_PLUGIN_VISUALIZATION_HH_
19 #define IGNITION_GAZEBO_SYSTEMS_OPTICAL_TACTILE_PLUGIN_VISUALIZATION_HH_
20 
21 #include <memory>
22 #include <string>
23 
24 #include <gz/sim/config.hh>
25 #include <gz/sim/System.hh>
26 #include <gz/msgs/marker.pb.h>
27 
29 
30 namespace ignition
31 {
32 namespace gazebo
33 {
34 // Inline bracket to help doxygen filtering.
35 inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
36 namespace systems
37 {
38 namespace optical_tactile_sensor
39 {
40  // This class handles the configuration and process of visualizing the
41  // different elements needed for the Optical Tactile Sensor plugin.
42  // Terminology:
43  // "Contacts" refers to data from the contact sensor based on physics.
44  // "Normal forces" refers to post-processed data from the depth camera based
45  // purely on imagery.
46  // These two sets of data are currently disjoint and visualized separately.
48  {
56  std::string &_modelName,
57  ignition::math::Vector3d &_sensorSize,
58  double &_forceLength,
59  float &_cameraUpdateRate,
60  ignition::math::Pose3f &_depthCameraOffset);
61 
65  private: void InitializeSensorMarkerMsg(
66  ignition::msgs::Marker &_sensorMarkerMsg);
67 
72  public: void RequestSensorMarkerMsg(
73  ignition::math::Pose3f const &_sensorPose);
74 
78  private: void InitializeContactsMarkerMsg(
79  ignition::msgs::Marker &_contactsMarkerMsg);
80 
85  public: void AddContactToMarkerMsg(
86  ignition::msgs::Contact const &_contact,
87  ignition::msgs::Marker &_contactsMarkerMsg);
88 
91  public: void RequestContactsMarkerMsg(
92  components::ContactSensorData const *_contacts);
93 
99  private: void InitializeNormalForcesMarkerMsgs(
100  ignition::msgs::Marker &_positionMarkerMsg,
101  ignition::msgs::Marker &_forceMarkerMsg);
102 
115  public: void AddNormalForceToMarkerMsgs(
116  ignition::msgs::Marker &_positionMarkerMsg,
117  ignition::msgs::Marker &_forceMarkerMsg,
118  ignition::math::Vector3f &_position,
119  ignition::math::Vector3f &_normalForce,
120  ignition::math::Pose3f &_sensorWorldPose);
121 
128  public: void RequestNormalForcesMarkerMsgs(
129  ignition::msgs::Marker &_positionMarkerMsg,
130  ignition::msgs::Marker &_forceMarkerMsg);
131 
133  public: void RemoveNormalForcesAndContactsMarkers();
134 
136  private: ignition::transport::Node node;
137 
139  private: std::string modelName;
140 
142  private: ignition::math::Vector3d sensorSize;
143 
145  private: double forceLength;
146 
148  private: float cameraUpdateRate;
149 
151  private: ignition::math::Pose3f depthCameraOffset;
152 
154  private: bool normalForcesMsgsAreInitialized{false};
155  };
156 } // namespace optical_tactile_sensor
157 } // namespace systems
158 } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE
159 } // namespace gazebo
160 } // namespace ignition
161 
162 #endif
This library is part of the Gazebo project.
STL class.
void AddNormalForceToMarkerMsgs(ignition::msgs::Marker &_positionMarkerMsg, ignition::msgs::Marker &_forceMarkerMsg, ignition::math::Vector3f &_position, ignition::math::Vector3f &_normalForce, ignition::math::Pose3f &_sensorWorldPose)
Create a marker messages representing the normal force computed from depth camera.
OpticalTactilePluginVisualization(std::string &_modelName, ignition::math::Vector3d &_sensorSize, double &_forceLength, float &_cameraUpdateRate, ignition::math::Pose3f &_depthCameraOffset)
Constructor.
A component type that wraps any data type. The intention is for this class to be used to create simpl...
Definition: gz/sim/components/Component.hh:324
void RequestSensorMarkerMsg(ignition::math::Pose3f const &_sensorPose)
Request the "/marker" service for the sensor marker. This can be helpful when debbuging,...
void RequestContactsMarkerMsg(components::ContactSensorData const *_contacts)
Request the "/marker" service for the contacts marker.
void AddContactToMarkerMsg(ignition::msgs::Contact const &_contact, ignition::msgs::Marker &_contactsMarkerMsg)
Add a contact to the marker message representing the contacts from the contact sensor based on physic...
void RemoveNormalForcesAndContactsMarkers()
Remove all normal forces and contact markers.
void RequestNormalForcesMarkerMsgs(ignition::msgs::Marker &_positionMarkerMsg, ignition::msgs::Marker &_forceMarkerMsg)
Request the "/marker" service for the marker messages representing the normal forces.