Select Git revision
DummyConnector.cpp
DummyConnector.cpp 1.76 KiB
//
// Created by Johannes Mey on 17/01/2021.
//
#include <ros/ros.h>
#include <cmath>
#include "DummyConnector.h"
#include "Connector.h"
bool DummyConnector::pickAndDrop(Object &robot, Object &object, Object &bin, bool simulateOnly) {
ROS_INFO_STREAM("[ros2cgv-dummy] \"Picking and dropping\" for 3 seconds...");
bool result = Connector::pickAndDrop(robot, object, bin, simulateOnly);
ros::Rate(ros::Duration(3)).sleep();
sendScene();
return result;
}
bool DummyConnector::pickAndPlace(Object &robot, Object &object, Object &location, bool simulateOnly) {
ROS_INFO_STREAM("[ros2cgv-dummy] \"Picking and placing\" for 3 seconds...");
bool result = Connector::pickAndPlace(robot, object, location, simulateOnly);
ros::Rate(ros::Duration(3)).sleep();
return result;
}
bool DummyConnector::reachableLocation(const Object &robot, const Object &location, const Object &object) {
// we pretend the location is an object and try to reach it
return reachableObject(robot, location);
}
bool DummyConnector::reachableObject(const Object &robot, const Object &object) {
double dx = object.pos().x() - robot.pos().x();
double dy = object.pos().y() - robot.pos().y();
double dz = object.pos().z() - robot.pos().z();
// if the location is within a "cone" with a radius of 50mm around the robot base, it is too close to place
if (std::sqrt(dx*dx + dy*dy) < 0.05) {
return false;
}
// if the location is more than 750mm away from the robot base it is too far away
if (std::sqrt(dx*dx + dy*dy + dz*dz) > 0.75) {
return false;
}
// otherwise we can reach it
return true;
}
DummyConnector::DummyConnector(const ros::NodeHandle &nodeHandle, const std::string &cellName) : Connector(nodeHandle, cellName) {}