visualization_meshcat.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2019
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 //
8 // * Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of nor the names of its contributors may be used to
14 // endorse or promote products derived from this software without specific
15 // prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 
30 #ifdef MSGPACK_FOUND
31 #include <chrono>
32 #include <regex>
33 
36 
37 // Timeout in ms
38 #define TIMEOUT 10000
39 
40 namespace exotica
41 {
42 inline std::vector<double> FrameToVector(const KDL::Frame& frame, double scale_x = 1.0, double scale_y = 1.0, double scale_z = 1.0)
43 {
44  std::vector<double> ret(16);
45  ret[0] = frame.M.data[0] * scale_x;
46  ret[1] = frame.M.data[3];
47  ret[2] = frame.M.data[6];
48  ret[3] = 0.0;
49  ret[4] = frame.M.data[1];
50  ret[5] = frame.M.data[4] * scale_y;
51  ret[6] = frame.M.data[7];
52  ret[7] = 0.0;
53  ret[8] = frame.M.data[2];
54  ret[9] = frame.M.data[5];
55  ret[10] = frame.M.data[8] * scale_z;
56  ret[11] = 0.0;
57  ret[12] = frame.p.data[0];
58  ret[13] = frame.p.data[1];
59  ret[14] = frame.p.data[2];
60  ret[15] = 1.0;
61  return ret;
62 };
63 
64 inline std::vector<double> PositionToVector(const KDL::Frame& frame)
65 {
66  std::vector<double> ret(3);
67  ret[0] = frame.p.data[0];
68  ret[1] = frame.p.data[1];
69  ret[2] = frame.p.data[2];
70  return ret;
71 };
72 
73 inline std::vector<double> QuaternionToVector(const KDL::Frame& frame)
74 {
75  std::vector<double> ret(4);
76  frame.M.GetQuaternion(ret[0], ret[1], ret[2], ret[3]);
77  return ret;
78 };
79 
80 VisualizationMeshcat::VisualizationMeshcat(ScenePtr scene, const std::string& url, bool use_mesh_materials, const std::string& file_url) : scene_(scene), context_(1), zmq_url_(url), file_url_(file_url)
81 {
82  HIGHLIGHT_NAMED("VisualizationMeshcat", "Initialising visualizer");
83  Initialize(use_mesh_materials);
84 }
86 
87 void VisualizationMeshcat::Initialize(bool use_mesh_materials)
88 {
89  // Connecting twice as per comment at:
90  // https://github.com/rdeits/meshcat-python/blob/aa3865143120f5ace8e62aab71d825e33674d277/src/meshcat/visualizer.py#L60
91  ConnectZMQ();
93  if (file_url_ == "")
94  {
95  std::regex url_regex("(.*):(?:\\d+)(?:\\/static\\/)");
96  std::smatch match;
97  if (std::regex_search(web_url_, match, url_regex) && match.size() > 1)
98  {
99  file_url_ = match.str(1) + ":9000/files/";
100  }
101  }
102 
103  if (web_url_.size() > 7) file_url_ = web_url_.substr(0, web_url_.size() - 7) + "files/";
104  ConnectZMQ();
105  path_prefix_ = "/exotica/" + scene_->GetName() + "/";
106 }
107 
109 {
110  socket_ = std::unique_ptr<zmq::socket_t>(new zmq::socket_t(context_, ZMQ_REQ));
111  socket_->setsockopt(ZMQ_RCVTIMEO, TIMEOUT);
112  socket_->connect(zmq_url_);
113 }
114 
115 void VisualizationMeshcat::SendZMQ(const std::string& data)
116 {
117  zmq::message_t request(data.size());
118  std::memcpy(request.data(), data.c_str(), data.size());
119  socket_->send(request);
120 }
121 
123 {
124  char buffer[2048];
125  std::memset(buffer, 0, 2048);
126  socket_->recv(&buffer, 2048);
127  return std::string(buffer);
128 }
129 
131 {
132  SendZMQ("url");
133  return ReceiveZMQ();
134 }
135 
137 {
138  return web_url_;
139 }
140 
142 {
143  return file_url_;
144 }
145 
146 template <typename T>
148 {
149  msgpack::sbuffer sbuf;
150  msgpack::pack(sbuf, msg);
151  socket_->send(msg.type.data(), msg.type.size(), ZMQ_SNDMORE);
152  socket_->send(msg.path.data(), msg.path.size(), ZMQ_SNDMORE);
153  socket_->send(sbuf.data(), sbuf.size());
154  ReceiveZMQ();
155 }
156 
157 void VisualizationMeshcat::DisplayScene(bool use_mesh_materials)
158 {
159  const std::vector<std::weak_ptr<KinematicElement>>& elements = scene_->GetKinematicTree().GetTree();
160 
161  for (std::weak_ptr<KinematicElement> weak_element : elements)
162  {
163  std::shared_ptr<KinematicElement> element = weak_element.lock();
164  if (element->visual.size() == 0) continue;
165  for (auto visual : element->visual)
166  {
167  if (!visual.shape) continue;
168  switch (visual.shape->type)
169  {
170  case shapes::SPHERE:
171  {
172  std::shared_ptr<shapes::Sphere> sphere = std::static_pointer_cast<shapes::Sphere>(ToStdPtr(visual.shape));
173 
174  auto object = visualization::SetObject(path_prefix_ + visual.name,
175  visualization::CreateGeometryObject(visualization::GeometrySphere(sphere->radius),
176  visualization::Material(visualization::RGB(visual.color(0), visual.color(1), visual.color(2)), visual.color(3))));
177  object.object.object.matrix = FrameToVector(visual.frame);
178  SendMsg(object);
179  SendMsg(visualization::SetTransform(object.path, FrameToVector(element->frame)));
180  }
181  break;
182  case shapes::BOX:
183  {
184  std::shared_ptr<shapes::Box> box = std::static_pointer_cast<shapes::Box>(ToStdPtr(visual.shape));
185 
186  auto object = visualization::SetObject(path_prefix_ + visual.name,
187  visualization::CreateGeometryObject(visualization::GeometryBox(box->size[0], box->size[1], box->size[2]),
188  visualization::Material(visualization::RGB(visual.color(0), visual.color(1), visual.color(2)), visual.color(3))));
189  object.object.object.matrix = FrameToVector(visual.frame);
190  SendMsg(object);
191  SendMsg(visualization::SetTransform(object.path, FrameToVector(element->frame)));
192  }
193  break;
194  case shapes::CYLINDER:
195  {
196  std::shared_ptr<shapes::Cylinder> cylinder = std::static_pointer_cast<shapes::Cylinder>(ToStdPtr(visual.shape));
197 
198  auto object = visualization::SetObject(path_prefix_ + visual.name,
199  visualization::CreateGeometryObject(visualization::GeometryCylinder(cylinder->radius, cylinder->length),
200  visualization::Material(visualization::RGB(visual.color(0), visual.color(1), visual.color(2)), visual.color(3))));
201  // Rotate the cylinder to match meshcat convention
202  object.object.object.matrix = FrameToVector(visual.frame * KDL::Frame(KDL::Rotation::RotX(M_PI_2)));
203  SendMsg(object);
204  SendMsg(visualization::SetTransform(object.path, FrameToVector(element->frame)));
205  }
206  break;
207  default:
208  {
209  if (!visual.shape_resource_path.empty())
210  {
211  auto mesh = visualization::GeometryMesh(visual.shape_resource_path, file_url_ + visual.shape_resource_path);
212  // If using STL files or if requested specifically, use URDF colours
213  if (!use_mesh_materials || mesh.format == "stl")
214  {
216  visualization::Material(visualization::RGB(visual.color(0), visual.color(1), visual.color(2)), visual.color(3))));
217  object.object.object.matrix =
218  FrameToVector(visual.frame, visual.scale(0), visual.scale(1), visual.scale(2));
219  SendMsg(object);
220  SendMsg(visualization::SetTransform(object.path, FrameToVector(element->frame)));
221  }
222  else
223  {
224  auto object = visualization::SetObject(path_prefix_ + visual.name,
226  visualization::Material(visualization::RGB(visual.color(0), visual.color(1), visual.color(2)), visual.color(3))));
227  object.object.object.matrix =
228  FrameToVector(visual.frame, visual.scale(0), visual.scale(1), visual.scale(2));
229  SendMsg(object);
230  SendMsg(visualization::SetTransform(object.path, FrameToVector(element->frame)));
231  }
232  }
233  else
234  {
235  if (visual.shape->type == shapes::MESH)
236  {
237  std::shared_ptr<shapes::Mesh> shape = std::static_pointer_cast<shapes::Mesh>(visual.shape);
238  auto mesh = visualization::GeometryMeshBuffer(visual.shape);
240  visualization::Material(visualization::RGB(visual.color(0), visual.color(1), visual.color(2)), visual.color(3))));
241  object.object.object.matrix =
242  FrameToVector(visual.frame, visual.scale(0), visual.scale(1), visual.scale(2));
243  SendMsg(object);
244  SendMsg(visualization::SetTransform(object.path, FrameToVector(element->frame)));
245  }
246  else
247  {
248  HIGHLIGHT("Unsupported shape! " << element->segment.getName());
249  }
250  }
251  }
252  break;
253  }
254  }
255  }
256 }
257 
259 {
260  const std::vector<std::weak_ptr<KinematicElement>>& elements = scene_->GetKinematicTree().GetTree();
261  scene_->Update(state, t);
262 
263  for (std::weak_ptr<KinematicElement> weak_element : elements)
264  {
265  std::shared_ptr<KinematicElement> element = weak_element.lock();
266  if (element->visual.size() == 0) continue;
267  for (auto visual : element->visual)
268  {
269  SendMsg(visualization::SetTransform(path_prefix_ + visual.name, FrameToVector(element->frame)));
270  }
271  }
272 }
273 
275 {
276  auto set_animation = visualization::SetAnimation();
277 
278  std::string clip_name = "default";
279  double fps = 1.0 / dt;
280 
281  const std::vector<std::weak_ptr<KinematicElement>>& elements = scene_->GetKinematicTree().GetTree();
282  for (std::weak_ptr<KinematicElement> weak_element : elements)
283  {
284  std::shared_ptr<KinematicElement> element = weak_element.lock();
285  if (element->visual.size() == 0) continue;
286  for (auto visual : element->visual)
287  {
288  visualization::Animation anim(path_prefix_ + visual.name);
289  anim.clip = visualization::Clip(fps, clip_name);
290  anim.clip.tracks.resize(2);
291  anim.clip.tracks[0] = visualization::Track(".position", "vector3");
292  anim.clip.tracks[1] = visualization::Track(".quaternion", "quaternion");
293  anim.clip.tracks[0].keys.reserve(trajectory.rows());
294  anim.clip.tracks[1].keys.reserve(trajectory.rows());
295  set_animation.animations.push_back(anim);
296  }
297  }
298 
299  for (int t = 0; t < trajectory.rows(); ++t)
300  {
301  double time = static_cast<double>(t) * dt;
302  scene_->Update(trajectory.row(t), time);
303 
304  int i = 0;
305  for (std::weak_ptr<KinematicElement> weak_element : elements)
306  {
307  std::shared_ptr<KinematicElement> element = weak_element.lock();
308  if (element->visual.size() == 0) continue;
309  for (auto visual : element->visual)
310  {
311  set_animation.animations[i].clip.tracks[0].keys.push_back(visualization::Key(time, PositionToVector(element->frame)));
312  set_animation.animations[i].clip.tracks[1].keys.push_back(visualization::Key(time, QuaternionToVector(element->frame)));
313  ++i;
314  }
315  }
316  }
317 
318  SendMsg(set_animation);
319 }
320 
321 void VisualizationMeshcat::Delete(const std::string& path)
322 {
323  SendMsg(visualization::Delete("/exotica/" + path));
324 }
325 
326 void VisualizationMeshcat::SetProperty(const std::string& path, const std::string& property, const double& value)
327 {
328  SendMsg(visualization::Property<double>(path, property, value));
329 }
330 
331 void VisualizationMeshcat::SetProperty(const std::string& path, const std::string& property, const std::string& value)
332 {
333  SendMsg(visualization::Property<std::string>(path, property, value));
334 }
335 
336 void VisualizationMeshcat::SetProperty(const std::string& path, const std::string& property, const bool& value)
337 {
338  SendMsg(visualization::Property<bool>(path, property, value));
339 }
340 
341 void VisualizationMeshcat::SetProperty(const std::string& path, const std::string& property, const Eigen::Vector3d& value)
342 {
343  std::vector<double> val(3);
344  val[0] = value(0);
345  val[1] = value(1);
346  val[2] = value(2);
347  SendMsg(visualization::Property<std::vector<double>>(path, property, val));
348 }
349 
350 void VisualizationMeshcat::SetProperty(const std::string& path, const std::string& property, const Eigen::Vector4d& value)
351 {
352  std::vector<double> val(4);
353  val[0] = value(0);
354  val[1] = value(1);
355  val[2] = value(2);
356  val[3] = value(3);
357  SendMsg(visualization::Property<std::vector<double>>(path, property, val));
358 }
359 } // namespace exotica
360 #endif // MSGPACK_FOUND
exotica::VisualizationMeshcat::GetFileURL
std::string GetFileURL()
exotica::VisualizationMeshcat::zmq_url_
std::string zmq_url_
Definition: visualization_meshcat.h:72
exotica::visualization::CreateGeometryObject
Object< T > CreateGeometryObject(const T &geometry_in, const Material &material_in=Material(), const std::string &uuid_in="")
Definition: visualization_meshcat_types.h:391
ToStdPtr
std::shared_ptr< T > ToStdPtr(const boost::shared_ptr< T > &p)
Definition: tools.h:148
exotica::VisualizationMeshcat::RequestWebURL
std::string RequestWebURL()
exotica::VisualizationMeshcat::ConnectZMQ
void ConnectZMQ()
shapes::SPHERE
SPHERE
exotica::VisualizationMeshcat::ReceiveZMQ
std::string ReceiveZMQ()
exotica
Definition: collision_scene.h:46
exotica::VisualizationMeshcat::file_url_
std::string file_url_
Definition: visualization_meshcat.h:74
visualization_meshcat_types.h
shapes::MESH
MESH
exotica::VisualizationMeshcat::SendMsg
void SendMsg(T msg)
HIGHLIGHT_NAMED
#define HIGHLIGHT_NAMED(name, x)
Definition: printable.h:62
exotica::VisualizationMeshcat::SetProperty
void SetProperty(const std::string &path, const std::string &property, const double &value)
exotica::ScenePtr
std::shared_ptr< Scene > ScenePtr
Definition: scene.h:246
exotica::visualization::SetObject
SetObjectType< T > SetObject(const std::string &path_in, const T &object_in)
Definition: visualization_meshcat_types.h:184
Eigen::VectorXdRefConst
const typedef Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
Convenience wrapper for storing references to sub-matrices/vectors.
Definition: conversions.h:53
HIGHLIGHT
#define HIGHLIGHT(x)
Definition: printable.h:61
exotica::VisualizationMeshcat::VisualizationMeshcat
VisualizationMeshcat(ScenePtr scene, const std::string &url, bool use_mesh_materials=true, const std::string &file_url="")
exotica::VisualizationMeshcat::DisplayTrajectory
void DisplayTrajectory(Eigen::MatrixXdRefConst trajectory, double dt=1.0)
Eigen::MatrixXdRefConst
const typedef Eigen::Ref< const Eigen::MatrixXd > & MatrixXdRefConst
Definition: conversions.h:54
exotica::VisualizationMeshcat::web_url_
std::string web_url_
Definition: visualization_meshcat.h:73
visualization_meshcat.h
exotica::VisualizationMeshcat::socket_
std::unique_ptr< zmq::socket_t > socket_
Definition: visualization_meshcat.h:79
exotica::VisualizationMeshcat::DisplayScene
void DisplayScene(bool use_mesh_materials=true)
exotica::VisualizationMeshcat::Initialize
void Initialize(bool use_mesh_materials)
exotica::VisualizationMeshcat::~VisualizationMeshcat
virtual ~VisualizationMeshcat()
exotica::visualization::CreateMeshObject
MeshObject< T > CreateMeshObject(const T &geometry_in, const Material &material_in=Material(), const std::string &uuid_in="")
Definition: visualization_meshcat_types.h:404
shapes::CYLINDER
CYLINDER
exotica::VisualizationMeshcat::path_prefix_
std::string path_prefix_
Definition: visualization_meshcat.h:76
exotica::VisualizationMeshcat::context_
zmq::context_t context_
Definition: visualization_meshcat.h:78
exotica::VisualizationMeshcat::SendZMQ
void SendZMQ(const std::string &data)
exotica::VisualizationMeshcat::scene_
ScenePtr scene_
Definition: visualization_meshcat.h:62
exotica::VisualizationMeshcat::GetWebURL
std::string GetWebURL()
exotica::visualization::RGB
long RGB(double R, double G, double B)
Definition: visualization_meshcat_types.h:57
shapes::BOX
BOX
exotica::VisualizationMeshcat::DisplayState
void DisplayState(Eigen::VectorXdRefConst state, double t=0.0)
t
geometry_msgs::TransformStamped t
exotica::VisualizationMeshcat::Delete
void Delete(const std::string &path="")


exotica_core
Author(s): Yiming Yang, Michael Camilleri
autogenerated on Fri Aug 2 2024 08:43:02