rviz_robot_builder.h
Go to the documentation of this file.
1 /******************************************************************************
2 Copyright (c) 2017, Alexander W. Winkler. All rights reserved.
3 
4 Redistribution and use in source and binary forms, with or without
5 modification, are permitted provided that the following conditions are met:
6 
7 * Redistributions of source code must retain the above copyright notice, this
8  list of conditions and the following disclaimer.
9 
10 * Redistributions in binary form must reproduce the above copyright notice,
11  this list of conditions and the following disclaimer in the documentation
12  and/or other materials provided with the distribution.
13 
14 * Neither the name of the copyright holder nor the names of its
15  contributors may be used to endorse or promote products derived from
16  this software without specific prior written permission.
17 
18 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
22 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 ******************************************************************************/
29 
30 #ifndef XPP_VIS_RVIZ_ROBOT_BUILDER_H_
31 #define XPP_VIS_RVIZ_ROBOT_BUILDER_H_
32 
33 #include <string>
34 #include <vector>
35 
36 #include <visualization_msgs/MarkerArray.h>
37 
38 #include <xpp_msgs/RobotParameters.h>
39 #include <xpp_msgs/RobotStateCartesian.h>
40 #include <xpp_msgs/TerrainInfo.h>
41 
42 #include <xpp_states/state.h>
44 
45 
46 namespace xpp {
47 
57 public:
58  using Marker = visualization_msgs::Marker;
59  using MarkerVec = std::vector<Marker>;
60  using MarkerArray = visualization_msgs::MarkerArray;
61 
67 
68 public:
73  virtual ~RvizRobotBuilder () = default;
74 
80  MarkerArray BuildRobotState(const xpp_msgs::RobotStateCartesian& msg) const;
81 
90  void SetRobotParameters(const xpp_msgs::RobotParameters& msg);
91 
100  void SetTerrainParameters(const xpp_msgs::TerrainInfo& msg);
101 
102 private:
103  // various modular functions that are stitched together to generate the
104  // robot state.
105  // pos_W = position expressed in world frame
106  // f_W = forces expressed in world frame.
107  // c = which leg is currently in contact with the environment.
108  MarkerVec CreateEEPositions(const EEPos& pos_W,
109  const ContactState& c) const;
110  MarkerVec CreateEEForces(const EEForces& f_W,
111  const EEPos& pos_W,
112  const ContactState& c) const;
113  MarkerVec CreateFrictionCones(const EEPos& pos_W,
114  const ContactState& c) const;
116  const EEPos& pos_W) const;
117  MarkerVec CreateRangeOfMotion(const State3d& base) const;
118  Marker CreateGravityForce (const Vector3d& base_pos) const;
119  Marker CreateBasePose(const Vector3d& pos,
120  Eigen::Quaterniond ori,
121  const ContactState& c) const;
122  Marker CreateCopPos(const EEForces& f_W,
123  const EEPos& pos_W) const;
124  Marker CreatePendulum(const Vector3d& base_pos,
125  const EEForces& f_W,
126  const EEPos& pos_W) const;
127  Marker CreateFrictionCone(const Vector3d& pos_W,
128  const Vector3d& terrain_normal,
129  double friction_coeff) const;
131  const Vector3d& pos) const;
132  Marker CreateSphere(const Vector3d& pos,
133  double diameter = 0.03) const;
134  Marker CreateBox(const Vector3d& pos, Eigen::Quaterniond ori,
135  const Vector3d& edge_length) const;
136 
137  xpp_msgs::RobotParameters params_msg_;
138  xpp_msgs::TerrainInfo terrain_msg_;
139 
140  const std::string frame_id_ = "world";
141 
147  void FillWithInvisible(int max_size, MarkerVec& vec) const;
148  const static int max_ee_ = 10; // maximum number of endeffectors
149 };
150 
151 } /* namespace xpp */
152 
153 #endif /* XPP_VIS_RVIZ_ROBOT_BUILDER_H_ */
void FillWithInvisible(int max_size, MarkerVec &vec) const
Marker CreateBasePose(const Vector3d &pos, Eigen::Quaterniond ori, const ContactState &c) const
Marker CreatePendulum(const Vector3d &base_pos, const EEForces &f_W, const EEPos &pos_W) const
Marker CreateCopPos(const EEForces &f_W, const EEPos &pos_W) const
Marker CreateForceArrow(const Vector3d &f, const Vector3d &pos) const
void SetTerrainParameters(const xpp_msgs::TerrainInfo &msg)
Additional information that can be used for visualization.
Marker CreateGravityForce(const Vector3d &base_pos) const
Marker CreateSphere(const Vector3d &pos, double diameter=0.03) const
xpp_msgs::RobotParameters params_msg_
Endeffectors< Eigen::Vector3d > EndeffectorsPos
void SetRobotParameters(const xpp_msgs::RobotParameters &msg)
Provides additional robot info that can be used for visualization.
Eigen::Vector3d Vector3d
MarkerVec CreateFrictionCones(const EEPos &pos_W, const ContactState &c) const
MarkerVec CreateRangeOfMotion(const State3d &base) const
MarkerVec CreateEEForces(const EEForces &f_W, const EEPos &pos_W, const ContactState &c) const
MarkerVec CreateEEPositions(const EEPos &pos_W, const ContactState &c) const
xpp_msgs::TerrainInfo terrain_msg_
virtual ~RvizRobotBuilder()=default
Marker CreateFrictionCone(const Vector3d &pos_W, const Vector3d &terrain_normal, double friction_coeff) const
Marker CreateBox(const Vector3d &pos, Eigen::Quaterniond ori, const Vector3d &edge_length) const
RvizRobotBuilder()
Builds an uninitialized visualizer.
visualization_msgs::MarkerArray MarkerArray
std::vector< Marker > MarkerVec
MarkerArray BuildRobotState(const xpp_msgs::RobotStateCartesian &msg) const
Constructs the RVIZ markers from the ROS message.
const std::string frame_id_
static const int max_ee_
visualization_msgs::Marker Marker
MarkerVec CreateSupportArea(const ContactState &c, const EEPos &pos_W) const
Constructs RVIZ markers that visualize a Cartesian robot state.


xpp_vis
Author(s): Alexander W. Winkler
autogenerated on Tue Dec 8 2020 03:10:32