Skip to content
Permalink
Browse files

Merge pull request #531 from ipab-slmc/wxm-fix-object-visualisation

Fix mesh object visualisation + color
  • Loading branch information...
wxmerkt committed Mar 14, 2019
2 parents e3a76da + df87ece commit 3305d49ee8a67953afb4bdf14747cef5083d4cf9
Showing with 10 additions and 10 deletions.
  1. +9 −9 exotica_core/src/kinematic_tree.cpp
  2. +1 −1 exotica_core/src/scene.cpp
@@ -620,35 +620,35 @@ void KinematicTree::PublishFrames()
marker_array_msg_.markers.clear();
for (int i = 0; i < tree_.size(); ++i)
{
if (tree_[i].lock()->shape && (!tree_[i].lock()->closest_robot_link.lock() || !tree_[i].lock()->closest_robot_link.lock()->is_robot_link))
if (tree_[i].lock()->shape_resource_path != "")
{
visualization_msgs::Marker mrk;
shapes::constructMarkerFromShape(tree_[i].lock()->shape.get(), mrk);
mrk.action = visualization_msgs::Marker::ADD;
mrk.frame_locked = true;
mrk.id = i;
mrk.ns = "CollisionObjects";
mrk.color = GetColor(tree_[i].lock()->color);
mrk.header.frame_id = "exotica/" + tree_[i].lock()->segment.getName();
mrk.pose.orientation.w = 1.0;
mrk.type = visualization_msgs::Marker::MESH_RESOURCE;
mrk.mesh_resource = tree_[i].lock()->shape_resource_path;
mrk.mesh_use_embedded_materials = true;
mrk.scale.x = tree_[i].lock()->scale(0);
mrk.scale.y = tree_[i].lock()->scale(1);
mrk.scale.z = tree_[i].lock()->scale(2);
marker_array_msg_.markers.push_back(mrk);
}
else if (tree_[i].lock()->shape_resource_path != "")
else if (tree_[i].lock()->shape && (!tree_[i].lock()->closest_robot_link.lock() || !tree_[i].lock()->closest_robot_link.lock()->is_robot_link))
{
visualization_msgs::Marker mrk;
shapes::constructMarkerFromShape(tree_[i].lock()->shape.get(), mrk);
mrk.action = visualization_msgs::Marker::ADD;
mrk.frame_locked = true;
mrk.id = i;
mrk.ns = "CollisionObjects";
mrk.color = GetColor(tree_[i].lock()->color);
mrk.header.frame_id = "exotica/" + tree_[i].lock()->segment.getName();
mrk.pose.orientation.w = 1.0;
mrk.type = visualization_msgs::Marker::MESH_RESOURCE;
mrk.mesh_resource = tree_[i].lock()->shape_resource_path;
mrk.mesh_use_embedded_materials = true;
mrk.scale.x = tree_[i].lock()->scale(0);
mrk.scale.y = tree_[i].lock()->scale(1);
mrk.scale.z = tree_[i].lock()->scale(2);
marker_array_msg_.markers.push_back(mrk);
}
}
@@ -547,7 +547,7 @@ void Scene::UpdateInternalFrames(bool update_request)
tf::transformKDLToEigen(it->segment.getFrameToTip(), pose);
std::string shape_resource_path = it->shape_resource_path;
Eigen::Vector3d scale = it->scale;
it = kinematica_.AddElement(it->segment.getName(), pose, it->parent_name, it->shape, it->segment.getInertia(), Eigen::Vector4d::Zero(), it->is_controlled);
it = kinematica_.AddElement(it->segment.getName(), pose, it->parent_name, it->shape, it->segment.getInertia(), it->color, it->is_controlled);
it->shape_resource_path = shape_resource_path;
it->scale = scale;
}

0 comments on commit 3305d49

Please sign in to comment.
You can’t perform that action at this time.