Fix marker creation: allow zero scale for geometric shapes (#430)

Boxes, spheres, and cylinders might explicitly have a zero size. Don't reset their scaling to (1,1,1)
This commit is contained in:
Robert Haschke 2023-02-23 20:42:23 +01:00 committed by GitHub
parent eae0bdc27f
commit ea776e35f6
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 5 additions and 6 deletions

View File

@ -122,6 +122,8 @@ void generateMarkers(const moveit::core::RobotState& robot_state, const MarkerCa
auto element_handler = [&](const T& element) {
if (element && element->geometry) {
createGeometryMarker(m, *element->geometry, element->origin, materialColor(*model, materialName(*element)));
if (m.scale.x == 0 && m.scale.y == 0 && m.scale.z == 0)
return; // skip zero-size marker
m.pose = rviz_marker_tools::composePoses(robot_state.getGlobalLinkTransform(name), m.pose);
callback(m, name);
valid_found = true;

View File

@ -158,12 +158,6 @@ void prepareMarker(vm::Marker& m, int marker_type) {
m.points.clear();
m.colors.clear();
// ensure valid scale
if (m.scale.x == 0 && m.scale.y == 0 && m.scale.z == 0) {
m.scale.x = 1.0;
m.scale.y = 1.0;
m.scale.z = 1.0;
}
// ensure non-null orientation
if (m.pose.orientation.w == 0 && m.pose.orientation.x == 0 && m.pose.orientation.y == 0 && m.pose.orientation.z == 0)
m.pose.orientation.w = 1.0;
@ -188,6 +182,7 @@ vm::Marker& makeXYPlane(vm::Marker& m) {
p[3].y = -1.0;
p[3].z = 0.0;
m.scale.x = m.scale.y = m.scale.z = 1.0;
prepareMarker(m, vm::Marker::TRIANGLE_LIST);
m.points.push_back(p[0]);
m.points.push_back(p[1]);
@ -217,6 +212,7 @@ vm::Marker& makeYZPlane(vm::Marker& m) {
/// create a cone of given angle along the x-axis
vm::Marker makeCone(double angle, vm::Marker& m) {
m.scale.x = m.scale.y = m.scale.z = 1.0;
prepareMarker(m, vm::Marker::TRIANGLE_LIST);
geometry_msgs::Point p[3];
p[0].x = p[0].y = p[0].z = 0.0;
@ -296,6 +292,7 @@ vm::Marker& makeArrow(vm::Marker& m, double scale, bool tip_at_origin) {
}
vm::Marker& makeText(vm::Marker& m, const std::string& text) {
m.scale.x = m.scale.y = m.scale.z = 1.0;
prepareMarker(m, vm::Marker::TEXT_VIEW_FACING);
m.text = text;
return m;