diff --git a/src/moveit_sorting_controller.cpp b/src/moveit_sorting_controller.cpp index 1a4ae2c2321414182fdf69648eb2e080f0bc78ab..e5ba294a303d324bc487dd832a61fcbd426a1740 100644 --- a/src/moveit_sorting_controller.cpp +++ b/src/moveit_sorting_controller.cpp @@ -37,6 +37,16 @@ int main(int argc, char** argv) MoveItRobotArmController controller{ n, NODE_NAME, robotName }; + // we can only set an absolute workspace here, because we do not have the scene yet! + geometry_msgs::Point minPoint, maxPoint; + minPoint.x = -.58; // we need 58 free space, i.e. ~40 cm behind the table (if the robot is at -22) + minPoint.y = -1; + minPoint.z = 0; // the robot can not reach under itself, i.e., into the table + maxPoint.x = 1; + maxPoint.y = 1; + maxPoint.z = 1.5; + controller.setWorkspace(minPoint, maxPoint, true); + auto mqttServer = getPrivateParameter<std::string>("mqtt_server", "localhost:1883"); std::unique_ptr<MqttConnection> mqtt_connection = std::make_unique<MqttConnection>(mqttServer, ros::this_node::getName());