mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
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:
parent
eae0bdc27f
commit
ea776e35f6
@ -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;
|
||||
|
@ -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;
|
||||
|
Loading…
Reference in New Issue
Block a user