Skip to content
Snippets Groups Projects
Commit 9e717530 authored by Johannes Mey's avatar Johannes Mey
Browse files

limit the movit controller to not move more than 40cm behind the table

parent d1f50edd
Branches
No related tags found
No related merge requests found
...@@ -37,6 +37,16 @@ int main(int argc, char** argv) ...@@ -37,6 +37,16 @@ int main(int argc, char** argv)
MoveItRobotArmController controller{ n, NODE_NAME, robotName }; 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"); auto mqttServer = getPrivateParameter<std::string>("mqtt_server", "localhost:1883");
std::unique_ptr<MqttConnection> mqtt_connection = std::unique_ptr<MqttConnection> mqtt_connection =
std::make_unique<MqttConnection>(mqttServer, ros::this_node::getName()); std::make_unique<MqttConnection>(mqttServer, ros::this_node::getName());
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment