Skip to content
Snippets Groups Projects
Select Git revision
  • 41c5b7281719124526a5493cb18e049c10e9499e
  • master default protected
2 results

pose_storage_service.cpp

Blame
  • Forked from CeTI / ROS / ROS Packages / panda_teaching
    Source project has a limited visibility.
    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) {}