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

Merge branch 'noetic/feature/robotics-festival' into noetic/main

parents 9a442a0d 32800166
Branches
No related tags found
No related merge requests found
Showing
with 735 additions and 586 deletions
[submodule "lib/paho.mqtt.c"]
path = lib/paho.mqtt.c
url = https://github.com/eclipse/paho.mqtt.c.git
[submodule "lib/paho.mqtt.cpp"]
path = lib/paho.mqtt.cpp
url = https://github.com/eclipse/paho.mqtt.cpp.git
......@@ -17,6 +17,7 @@ find_package(catkin REQUIRED COMPONENTS
message_generation
panda_grasping
panda_util
paho_mqtt
)
## System dependencies are found with CMake's conventions
......@@ -117,10 +118,7 @@ catkin_package(
${Protobuf_INCLUDE_DIRS}
${CMAKE_CURRENT_BINARY_DIR}
${CMAKE_CURRENT_SOURCE_DIR}
lib/paho.mqtt.c/src
lib/paho.mqtt.cpp/src
LIBRARIES
${PROJECT_NAME}_scene_collision_object
${PROJECT_NAME}_base
${PROJECT_NAME}_moveit
${PROJECT_NAME}_nng
......@@ -130,14 +128,14 @@ catkin_package(
CATKIN_DEPENDS
message_runtime
moveit_ros_planning_interface
paho_mqtt
DEPENDS
nng
)
###########
## Build ##
###########
message(${catkin_INCLUDE_DIRS})
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
......@@ -146,33 +144,18 @@ include_directories(
${Protobuf_INCLUDE_DIRS}
${CMAKE_CURRENT_BINARY_DIR}
${CMAKE_CURRENT_SOURCE_DIR}
lib/paho.mqtt.c/src
lib/paho.mqtt.cpp/src
)
protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/connector.proto)
protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/Scene.proto)
find_package(PkgConfig REQUIRED)
add_subdirectory(lib/paho.mqtt.c ${CATKIN_DEVEL_PREFIX})
# for clion (on maybe catkin-make) change the C library location to
# ${CATKIN_DEVEL_PREFIX}/lib/libpaho-mqtt3a.so
# set(PAHO_MQTT_C_LIBRARIES "${CATKIN_DEVEL_PREFIX}/src/libpaho-mqtt3a.so")
set(PAHO_MQTT_C_LIBRARIES "${CATKIN_DEVEL_PREFIX}/lib/libpaho-mqtt3a.so")
set(PAHO_MQTT_C_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/lib/paho.mqtt.c/src")
add_subdirectory(lib/paho.mqtt.cpp)
add_dependencies(paho-mqttpp3 paho-mqtt3a)
## Declare a C++ library
add_library(${PROJECT_NAME}_base ${PROTO_SRCS} ${PROTO_HDRS} src/controller/DummyRobotArmController.cpp src/controller/RobotArmController.cpp src/controller/Controller.cpp)
add_library(${PROJECT_NAME}_moveit ${PROTO_SRCS} ${PROTO_HDRS} src/controller/MoveItRobotArmController.cpp)
add_library(${PROJECT_NAME}_nng ${PROTO_SRCS} ${PROTO_HDRS} src/connection/NngConnection.cpp)
add_library(${PROJECT_NAME}_mqtt ${PROTO_SRCS} ${PROTO_HDRS} src/connection/MqttConnection.cpp)
add_library(${PROJECT_NAME}_ros ${PROTO_SRCS} ${PROTO_HDRS} src/connection/RosConnection.cpp)
add_library(${PROJECT_NAME}_scene_collision_object src/util/scene_collision_object.cpp)
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
......@@ -183,7 +166,6 @@ add_dependencies(${PROJECT_NAME}_moveit ${${PROJECT_NAME}_EXPORTED_TARGETS} ${c
add_dependencies(${PROJECT_NAME}_nng ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(${PROJECT_NAME}_mqtt ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(${PROJECT_NAME}_ros ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(${PROJECT_NAME}_scene_collision_object ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
......@@ -207,7 +189,6 @@ target_link_libraries(${PROJECT_NAME}_ros
)
target_link_libraries(${PROJECT_NAME}_mqtt
${catkin_LIBRARIES}
paho-mqttpp3
)
target_link_libraries(${PROJECT_NAME}_base
${catkin_LIBRARIES}
......@@ -217,10 +198,6 @@ target_link_libraries(${PROJECT_NAME}_moveit
${catkin_LIBRARIES}
${Protobuf_LIBRARIES}
${PROJECT_NAME}_base
${PROJECT_NAME}_scene_collision_object
)
target_link_libraries(${PROJECT_NAME}_scene_collision_object
${catkin_LIBRARIES}
)
#############
......
......@@ -5,4 +5,4 @@ max_grasp_approach_velocity: 0.2
max_grasp_approach_acceleration: 0.2
max_grasp_transition_velocity: 0.2
max_grasp_transition_acceleration: 0.2
tud_grasp_force: 12.5
tud_grasp_force: 4
# this file overwrites the settings from the same file in the franka_control package
# arm_id: $(arg arm_id)
# joint_names:
# - $(arg arm_id)_joint1
# - $(arg arm_id)_joint2
# - $(arg arm_id)_joint3
# - $(arg arm_id)_joint4
# - $(arg arm_id)_joint5
# - $(arg arm_id)_joint6
# - $(arg arm_id)_joint7
# Configure the threshold angle for printing joint limit warnings.
# joint_limit_warning_threshold: 0.1 # [rad]
# Activate rate limiter? [true|false]
# rate_limiting: true
# Cutoff frequency of the low-pass filter. Set to >= 1000 to deactivate.
# cutoff_frequency: 100
# Internal controller for motion generators [joint_impedance|cartesian_impedance]
# internal_controller: joint_impedance
# Used to decide whether to enforce realtime mode [enforce|ignore]
realtime_config: ignore
# Configure the initial defaults for the collision behavior reflexes.
# collision_config:
# lower_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm]
# upper_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm]
# lower_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm]
# upper_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm]
# lower_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm]
# upper_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm]
# lower_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm]
# upper_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm]
# Publish joint states
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 100
panda_arm_controller:
type: position_controllers/JointTrajectoryController
joints:
- panda_joint1
- panda_joint2
- panda_joint3
- panda_joint4
- panda_joint5
- panda_joint6
- panda_joint7
constraints:
goal_time: 2.0
state_publish_rate: 100
# panda_hand_controller:
# type: position_controllers/JointTrajectoryController
# joints:
# - panda_finger_joint1
# - panda_finger_joint2
state_publish_rate: 25
......@@ -9,8 +9,8 @@
#include <functional>
/// This calls represents a connection that can publish and subscribe to channels
class Connection {
class Connection
{
protected:
/// callback defined by the initializeConnection() method
std::function<void(std::string, std::string)> messageReceivedCallback;
......@@ -20,11 +20,13 @@ public:
/// assumed that the connection listen to all channels
/// \param channel the connection should be listening on
/// \return true if the connection is able to listen to the channel
virtual bool listen(const std::string &channel) { return true; };
virtual bool listen(const std::string& channel)
{
return true;
};
/// Initializes the connection by specifying a callback for received messages. This method must be called exactly once.
/// \param callback to be called when a message is received
/// \return true if the initialization was successful
/// Initializes the connection by specifying a callback for received messages. This method must be called exactly
/// once. \param callback to be called when a message is received \return true if the initialization was successful
virtual bool initializeConnection(std::function<void(const std::string, const std::string)> callback) = 0;
/// Send a message over a channel
......
......@@ -7,37 +7,25 @@
#include "Connection.h"
#include <thread>
#include <mqtt/client.h>
class MqttConnection : virtual public Connection {
const std::string &mqtt_address;
const std::string &client_id;
std::unique_ptr<std::thread> mqtt_handler_thread;
mqtt::client client;
mqtt::string_collection topics;
class MqttConnection : virtual public Connection
{
std::vector<std::string> topics;
const int QOS = 0;
mqtt::async_client cli_;
mqtt::connect_options connOpts_;
public:
explicit MqttConnection(const std::string &mqttAddress, const std::string &client_id);
MqttConnection(const std::string& mqttAddress, const std::string& client_id);
bool send(const std::string& channel, const std::string& message) override;
bool initializeConnection(std::function<void(std::string, std::string)> callback) override;
virtual bool listen(const std::string &topic) override;
bool listen(const std::string& topic) override;
~MqttConnection();
private:
void receiveMessages();
mqtt::client &getClient();
bool ensureConnection();
};
#endif // CCF_MQTTCONNECTION_H
......@@ -13,16 +13,16 @@
/// A connection over NNG using Pair in version 1
/// see https://nng.nanomsg.org/man/v0.2.0/nng_pair.html
class NngConnection : public Connection {
class NngConnection : public Connection
{
const std::string& connection_address;
const bool server;
nng_socket sock{};
std::unique_ptr<std::thread> nng_receiver_thread;
std::string sendTopic;
std::string receiveTopic;
public:
public:
/// Constructor requiring an address as defined in https://nng.nanomsg.org/man/v1.3.2/nng.7.html
/// \param connection_address in server mode the port on which the connections listens for connections, in client
/// mode the address of the server
......@@ -52,5 +52,4 @@ public:
~NngConnection();
};
#endif // CCF_NNGCONNECTION_H
......@@ -10,8 +10,8 @@
#include <ros/ros.h>
#include <map>
class RosConnection : public Connection {
class RosConnection : public Connection
{
std::vector<std::string> ros_topics;
std::unique_ptr<std::thread> ros_spinner_thread;
std::vector<ros::Subscriber> ros_subscribers;
......@@ -21,7 +21,6 @@ class RosConnection : public Connection {
int output_queue_size;
public:
/**
* A connection to the ROS based communication, limited to std_messages/String.
* @param n a valid node handle
......@@ -44,7 +43,8 @@ public:
/**
* Adds a publisher, to be used to publish messages to a certain topic.
* Needs to be done because there is delay between registering a publisher, and the time where one can use the publisher.
* Needs to be done because there is delay between registering a publisher, and the time where one can use the
* publisher.
* @param channel the topic on which the publisher sends
* @return true if successfull added
*/
......@@ -56,7 +56,6 @@ public:
void receiveMessages();
~RosConnection();
};
#endif // SRC_ROSCONNECTION_H
......@@ -12,15 +12,15 @@
#include "ccf/connection/Connection.h"
#include <connector.pb.h>
class Controller {
#include <Scene.pb.h>
class Controller
{
private:
std::map<std::string, std::vector<std::function<void(const std::string)>>> callbacks;
std::map<std::string, std::vector<std::function<void(const std::string)>>> callbacks_;
protected:
ros::NodeHandle nodeHandle;
ros::NodeHandle node_handle_;
std::vector<std::shared_ptr<Connection>> connections;
void receive(const std::string& channel, const std::string& data);
......@@ -34,7 +34,6 @@ public:
void addCallback(const std::string& channel, const std::function<void(const std::string)>& function);
void sendToAll(const std::string& channel, const std::string& message);
};
#endif // CCF_CONTROLLER_H
......@@ -7,15 +7,20 @@
#include "RobotArmController.h"
class DummyRobotArmController : public virtual RobotArmController {
class DummyRobotArmController : public virtual RobotArmController
{
public:
explicit DummyRobotArmController(const ros::NodeHandle &nodeHandle, const std::string &cellName);
explicit DummyRobotArmController(const ros::NodeHandle& nodeHandle, const std::string& cellName,
const std::string& robotName);
bool pickAndDrop(Object& robot, Object& object, Object& bin, bool simulateOnly) override;
bool pickAndPlace(Object& robot, Object& object, Object& location, bool simulateOnly) override;
bool moveToPose(Object& robot, geometry_msgs::Pose pose, bool simulateOnly) override;
bool configureCollaborationZone(Object& zone, const std::string& owner) override;
/// Compute if a location is reachable by a robot, i.e., if an object can be placed or picked at this location
/// If the location is within a "cone" with a radius of 150mm around the robot base, it is too close to reach.
/// If the location is more than 750mm away from the robot base it is too far away.
......@@ -31,6 +36,9 @@ public:
/// \param object to be picked
/// \return true if the location is reachable
bool reachableObject(const Object& robot, const Object& object) override;
bool pick(const std::string& robot_name, const std::string& object_name, bool simulateOnly) override;
bool place(const std::string& robot_name, const std::string& location_name, bool simulateOnly) override;
bool drop(const std::string& robot_name, const std::string& bin_name, bool simulateOnly) override;
};
#endif // CCF_DUMMYROBOTARMCONTROLLER_H
//Dummy
// Created by Johannes Mey on 17/01/2021.
//
/*! \file MoveItRobotArmController.h
\author Johannes Mey
\author Sebastian Ebert
\date 17/01/2021
*/
#ifndef CCF_MOVEITROBOTARMCONTROLLER_H
#define CCF_MOVEITROBOTARMCONTROLLER_H
#include <mutex>
#include <std_msgs/String.h>
#include <tf2_ros/transform_listener.h>
......@@ -16,77 +21,120 @@
#include <google/protobuf/message.h>
#include "RobotArmController.h"
#include "ccf/util/scene_collision_object.h"
class MoveItRobotArmController : public virtual RobotArmController {
class MoveItRobotArmController : public virtual RobotArmController
{
private:
ros::NodeHandle n;
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
moveit::planning_interface::MoveGroupInterface group;
moveit::planning_interface::PlanningSceneInterface planning_scene_interface_;
moveit::planning_interface::MoveGroupInterface group_;
// std::string current_picked_object_id_; // TODO move this to the RobotArmController and use it in the lambdas
std::mutex planning_scene_update_in_progress_;
ros::ServiceServer get_scene_service;
/// position of the robot used to translate to moveit coordinates
/// TODO use pose instead of position
geometry_msgs::Point current_transform_;
std::map<std::string, SceneCollisionObject> collision_objects;
std::string current_picked_object_id; // TODO move this to the RobotArmController and use it in the lambdas
geometry_msgs::Point current_transform;
// TODO get rid of magic number
double open_amount = 0.078;
bool static_scene_elements_configured = false;
bool is_simulated_robot = false;
const double OPEN_AMOUNT = 0.078;
bool is_simulated_robot_ = false;
/**
* tf2 buffer to receive current robot state
*/
tf2_ros::Buffer tfBuffer;
tf2_ros::Buffer tf_buffer_;
public:
explicit MoveItRobotArmController(ros::NodeHandle &nodeHandle, const std::string &cellName);
/**
* Constructs an scene-update-object and updates it in the CCF controller.
* @return true if the scene has changed
*/
bool pushSceneUpdate();
virtual ~MoveItRobotArmController();
/**
* An object is physical if it may appear in the MoveitPlanningScene.
* This is true for standard objects (that are STATIONARY, i.e., not currently being moved), bins, boxes, and
* collaboration zones. Note that zones are physical, but might still be disabled!
* @param object a reference to the object
* @return true if the object is physical
*/
static bool isPhysical(const Object& object)
{
return object.type() == Object_Type_UNKNOWN || object.type() == Object_Type_BIN ||
(object.type() == Object_Type_BOX && object.state() == Object_State_STATE_STATIONARY) ||
object.type() == Object_Type_COLLABORATION_ZONE;
}
/** transform the pose from the scene to the moveit world
* FIXME it does not consider the orientation, which is assumed to be identical in both worlds
* @param scenePose
* @return
*/
geometry_msgs::Pose fromSceneToMoveIt(const geometry_msgs::Pose& scenePose) const;
bool pickAndDrop(Object &robot, Object &object, Object &bin, bool simulateOnly) override;
/**
* Get the support surface for an object
* @param object_to_pick the object of interest
* @return the id of the support surface or an empty string if non could be found
*/
std::string getSupportSurface(const moveit_msgs::CollisionObject& object_to_pick);
bool pickAndPlace(Object &robot, Object &object, Object &location, bool simulateOnly) override;
/**
* Get the support surface for an object when placed at a pose
* @param target_object the id object of interest
* @param pose the pose the object is considered to be at
* @return the id of the support surface or an empty string if non could be found
*/
std::string getSupportSurface(const std::string& target_object, const geometry_msgs::Pose& pose);
bool reachableLocation(const Object &robot, const Object &location, const Object &object) override;
/**
* Transform scene object to MoveIt collision object
* @param part the scene object
* @param robotPosition the robot position used to translate the coordinates to the MoveIt coordinate system
* @return a new collision object
*/
static moveit_msgs::CollisionObject
toCollisionObject(const Object& part, const geometry_msgs::Point& robotPosition,
moveit_msgs::CollisionObject::_operation_type operation = moveit_msgs::CollisionObject::ADD);
bool reachableObject(const Object &robot, const Object &object) override;
/**
* Transform scene object into the color of a MoveIt object
* @param part the scene object
* @return a new color object
*/
static moveit_msgs::ObjectColor toCollisionObjectColor(const Object& part);
bool loadScene(const std::string &sceneFile) override;
public:
explicit MoveItRobotArmController(ros::NodeHandle& nodeHandle, const std::string& cellName,
const std::string& robotName);
private:
virtual ~MoveItRobotArmController();
// from old controller
bool pickAndDrop(Object& robot, Object& object, Object& bin, bool simulate_only) override;
bool applyMoveItCollisionObjectList();
bool pickAndPlace(Object& robot, Object& object, Object& location, bool simulate_only) override;
/**
* Constructs an scene-update-object and updates it in the CCF controller.
* @return true on successful transmission
*/
bool pushSceneUpdate();
bool moveToPose(Object& robot, geometry_msgs::Pose pose, bool simulateOnly) override;
Scene applyInputTransformation(const Scene &scene);
bool configureCollaborationZone(Object& zone, const std::string& owner) override;
/**
* Picks a specified object
* @param objectId ID of the object to pick
*/
bool pick(const std::string &objectId);
bool reachableLocation(const Object& robot, const Object& location, const Object& object) override;
// FIXME this is not used right now, because we assume the robot to be at the center
geometry_msgs::Pose getBaseLinkPose();
bool reachableObject(const Object& robot, const Object& object) override;
bool place(geometry_msgs::Pose target_pose);
void initScene(const Scene& scene, bool sendUpdates) override;
bool pick(const std::string& robot_id, const std::string& object_id, bool simulate_only) override;
bool drop(const std::string& robot_id, const std::string& bin_id, bool simulate_only) override;
bool place(const std::string& robot_id, const std::string& location_name, bool simulate_only) override;
std::string attachedBox();
/**
* Places the currently selected object in the bin.
* @param bin_id the id of the bin
/** Specify the workspace bounding box with respect to the robot (not the world coordinate system).
*/
bool drop(const std::string &bin_id);
void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz);
std::string getSupportSurface(const moveit_msgs::CollisionObject &object_to_pick);
private:
bool doPick(const std::string& robot_id, const std::string& object_id, bool simulate_only, bool setIdleAfterwards);
};
#endif // CCF_MOVEITROBOTARMCONTROLLER_H
......@@ -9,28 +9,35 @@
#include <optional>
#include <ros/ros.h>
#include <geometry_msgs/Pose.h>
#include "ccf/connection/Connection.h"
#include "Controller.h"
#include <connector.pb.h>
class RobotArmController : public virtual Controller {
#include <Scene.pb.h>
class RobotArmController : public virtual Controller
{
private:
std::function<void(Selection)> selection_action_;
std::function<void()> scene_update_action_;
std::function<void(Command)> command_action_;
std::function<void(Selection)> selectionAction;
std::function<void()> sceneUpdateAction;
std::function<void(PickPlace)> pickPlaceAction;
std::string cell_name_;
bool removeObject(const Object &object);
std::string selection_topic_;
std::string init_scene_topic_;
std::string send_scene_topic_;
std::string send_delta_scene_topic_;
std::string command_topic_;
protected:
std::vector<std::unique_ptr<Connection>> connections_;
std::shared_ptr<Scene> scene_;
std::string robot_name_;
std::string cellName;
bool removeObject(const Object& object);
std::string selectionTopic;
std::string initSceneTopic;
std::string sendSceneTopic;
std::string commandTopic;
public:
const std::string& getSelectionTopic() const;
......@@ -44,31 +51,70 @@ public:
void setSendSceneTopic(const std::string& sendSceneTopic);
const std::string& getSendDeltaSceneTopic() const;
void setSendDeltaSceneTopic(const std::string& sendSceneTopic);
const std::string& getCommandTopic() const;
void setCommandTopic(const std::string& commandTopic);
protected:
ros::NodeHandle nodeHandle;
std::vector<std::unique_ptr<Connection>> connections;
std::shared_ptr<Scene> scene;
public:
virtual bool configureCollaborationZone(Object& zone, const std::string& owner);
const std::string& getRobotName() const;
void setRobotName(const std::string& robot_name);
RobotArmController(const ros::NodeHandle &nodeHandle, const std::string &cellName);
RobotArmController(const ros::NodeHandle& nodeHandle, const std::string& cellName, const std::string& robotName);
virtual bool loadScene(const std::string &sceneFile);
bool loadScene(const std::string& sceneFile);
std::shared_ptr<Scene> getScene();
void sendScene();
void initScene(const Scene &scene);
// application "skills"
void sendScene(const std::vector<std::string>& modified, const std::vector<std::string>& added,
const std::vector<std::string>& removed);
virtual void initScene(const Scene& scene, bool sendUpdates = true);
/// Pick an object
/// \param robot_name the robot to pick the object. If it is not controlled by this controller, the task is ignored.
/// \param object_name the name of the object to be picked
/// \param simulateOnly perform a dry-run only, do not actually execute the task
/// \return true if picking was successful
virtual bool pick(const std::string& robot_name, const std::string& object_name, bool simulateOnly) = 0;
/// Place an object onto a location
/// \param robot_name the robot to place the object. If it is not controlled by this controller, the task is ignored.
/// \param location_name the name of the location
/// \param simulateOnly perform a dry-run only, do not actually execute the task
/// \return true if placing was successful
virtual bool place(const std::string& robot_name, const std::string& location_name, bool simulateOnly) = 0;
/// Drop an object into a container
/// \param robot_name the robot to drop the object. If it is not controlled by this controller, the task is ignored.
/// \param bin_name the name of the container
/// \param simulateOnly perform a dry-run only, do not actually execute the task
/// \return true if the dropping was successful
virtual bool drop(const std::string& robot_name, const std::string& bin_name, bool simulateOnly) = 0;
/// A combination of the pick and the drop operations
/// \param robot
/// \param object
/// \param bin
/// \param simulateOnly
/// \return
virtual bool pickAndDrop(Object& robot, Object& object, Object& bin, bool simulateOnly);
virtual bool pickAndPlace(Object& robot, Object& object, Object& location, bool simulateOnly);
/// Evacuate a certain region
/// \param robot
/// \param space
/// \param simulateOnly
/// \return
virtual bool moveToPose(Object& robot, geometry_msgs::Pose pose, bool simulateOnly);
/// Compute if a location is reachable by a robot, i.e., if an object can be placed or picked at this location
/// \param robot
/// \param location
......@@ -82,15 +128,51 @@ public:
/// \return true if the location is reachable
virtual bool reachableObject(const Object& robot, const Object& object) = 0;
// reactive components: application logic provided by callbacks
void reactToSelectionMessage(std::function<void(Selection)> lambda);
/// Setter for a callback that is called when a selection message is sent to the scene
/// \param callback function that is given the received selection message
void reactToSelectionMessage(std::function<void(Selection)> callback);
void reactToPickAndPlaceMessage(std::function<void(PickPlace)> lambda);
/// Setter for a callback that is called when a command message is sent to the scene
/// \param callback function that is given the received command message
void reactToCommandMessage(std::function<void(Command)> callback);
void reactToSceneUpdateMessage(std::function<void()> lambda);
/// Setter for a callback that is called when a the scene is updated
/// \param callback function that is given the updated scene
void reactToSceneUpdateMessage(std::function<void()> callback);
// helper methods
/**
* resolve an object by name in the current scene
* @param the name of the object
* @return the object in the current scene
*/
Object* resolveObject(const std::string& id);
/**
* resolve an object by name in a given scene
* @param the name of the object
* @param scene a scene
* @return the object in the current scene
*/
static Object* resolveObject(const std::string& id, Scene& scene);
/**
* resolve an object by name in a given scene
* @param the name of the object
* @param scene a scene
* @return a const reference to the object in the provided scene
*/
static std::optional<const Object> resolveObject(const std::string& id, const Scene& scene);
static Scene loadSceneFromFile(const std::string& sceneFile);
/**
* Sends a delta update on the delta update channel
* @param added Objects in the current scene that have been newly added
* @param removed Objects no longer in the current scene
* @param changed Objects that have been modified in the current scene
*/
void sendDelta(const std::vector<std::string>& added, const std::vector<std::string>& removed,
const std::vector<std::string>& changed);
};
#endif // CCF_ROBOTARMCONTROLLER_H
......@@ -9,8 +9,8 @@
#include <sstream>
namespace CetiRosToolbox {
namespace CetiRosToolbox
{
/// Gets a parameter from the parameter server. If the parameter does not exist, the fallback is used. If no fallback
/// value is specified, an empty string is returned.
///
......@@ -22,23 +22,20 @@ namespace CetiRosToolbox {
/// \code{.cpp}
/// auto intValue1 = getParameter<int>(n, "foo"); // no fallback means no automatic matching
/// auto intValue2 = getParameter(n, "foo", 42); // the compiler matches T with int, this is okay
/// auto stringValue = getParameter<std::string>(n, "bar", "fourtytwo"); // const char* requires the std::string template parameter
/// \endcode
/// \tparam T a type supported by the parameter server.
/// The parameter server supports \c std::string double \c float \c int \c bool
/// and \c std::vector and \std::map of these types.
/// Additionally, also \c XmlRpc::XmlRpcValue is supported, though not as a vector or map.
/// \param n the NodeHandle, in the namespace of which the parameter is queried
/// \param key the name of the parameter
/// \param fallback the default value
/// \return the parameter (if it exists), otherwise the fallback value (if it exists), otherwise the default
/// auto stringValue = getParameter<std::string>(n, "bar", "fourtytwo"); // const char* requires the std::string
/// template parameter \endcode \tparam T a type supported by the parameter server. The parameter server supports \c
/// std::string double \c float \c int \c bool and \c std::vector and \std::map of these types. Additionally, also \c
/// XmlRpc::XmlRpcValue is supported, though not as a vector or map. \param n the NodeHandle, in the namespace of which
/// the parameter is queried \param key the name of the parameter \param fallback the default value \return the
/// parameter (if it exists), otherwise the fallback value (if it exists), otherwise the default
/// instance of the respective object
/// \warning Currently, this is a header-only library, but this might change in the future
template <class T>
T getParameter(const ros::NodeHandle &n, const std::string &key, T fallback = T()) {
if (!n.getParam(key, fallback)) {
ROS_WARN_STREAM("[" << ros::this_node::getName() << "] Could not get string value for "
<< n.getNamespace() << "/" << key
T getParameter(const ros::NodeHandle& n, const std::string& key, T fallback = T())
{
if (!n.getParam(key, fallback))
{
ROS_WARN_STREAM("Could not get value for " << n.getNamespace() << "/" << key
<< " from param server, using default '" << fallback << "'.");
}
return fallback;
......@@ -55,142 +52,152 @@ namespace CetiRosToolbox {
/// \code{.cpp}
/// auto intValue1 = getParameter<int>(n, "foo"); // no fallback means no automatic matching
/// auto intValue2 = getParameter(n, "foo", 42); // the compiler matches T with int, this is okay
/// auto stringValue = getParameter<std::string>(n, "bar", "fourtytwo"); // const char* requires the std::string template parameter
/// \endcode
/// \tparam T a type supported by the parameter server.
/// The parameter server supports \c std::string double \c float \c int \c bool
/// and \c std::vector and \std::map of these types.
/// Additionally, also \c XmlRpc::XmlRpcValue is supported, though not as a vector or map.
/// \param key the name of the parameter
/// \param fallback the default value
/// \return the parameter (if it exists), otherwise the fallback value (if it exists), otherwise the default
/// auto stringValue = getParameter<std::string>(n, "bar", "fourtytwo"); // const char* requires the std::string
/// template parameter \endcode \tparam T a type supported by the parameter server. The parameter server supports \c
/// std::string double \c float \c int \c bool and \c std::vector and \std::map of these types. Additionally, also \c
/// XmlRpc::XmlRpcValue is supported, though not as a vector or map. \param key the name of the parameter \param
/// fallback the default value \return the parameter (if it exists), otherwise the fallback value (if it exists),
/// otherwise the default
/// instance of the respective object
/// \warning Currently, this is a header-only library, but this might change in the future
template <class T>
T getPrivateParameter(const std::string &key, T fallback = T()) {
T getPrivateParameter(const std::string& key, T fallback = T())
{
ros::NodeHandle n("~"); // a private node handle
return getParameter(n, key, fallback);
}
/** @cond */ // the specializations are required to print the warning correctly
template <>
XmlRpc::XmlRpcValue getParameter(const ros::NodeHandle &n, const std::string &key, XmlRpc::XmlRpcValue fallback) {
if (!n.getParam(key, fallback)) {
ROS_WARN_STREAM("[" << ros::this_node::getName() << "] Could not get string value for "
<< n.getNamespace() << "/" << key
<< " from param server, using default '" << fallback.toXml() << "'.");
XmlRpc::XmlRpcValue getParameter(const ros::NodeHandle& n, const std::string& key, XmlRpc::XmlRpcValue fallback)
{
if (!n.getParam(key, fallback))
{
ROS_WARN_STREAM("Could not get string value for " << n.getNamespace() << "/" << key
<< " from param server, using default '" << fallback.toXml()
<< "'.");
}
return fallback;
}
template <>
std::vector<std::string>
getParameter(const ros::NodeHandle &n, const std::string &key, std::vector<std::string> fallback) {
if (!n.getParam(key, fallback)) {
std::vector<std::string> getParameter(const ros::NodeHandle& n, const std::string& key,
std::vector<std::string> fallback)
{
if (!n.getParam(key, fallback))
{
std::ostringstream vector;
std::copy(fallback.begin(), fallback.end(), std::ostream_iterator<std::string>(vector, " "));
ROS_WARN_STREAM("[" << ros::this_node::getName() << "] Could not get string value for "
<< n.getNamespace() << "/" << key
ROS_WARN_STREAM("[" << ros::this_node::getName() << "] Could not get value for " << n.getNamespace() << "/" << key
<< " from param server, using default [ " << vector.str() << "].");
}
return fallback;
}
template <>
std::vector<double> getParameter(const ros::NodeHandle &n, const std::string &key, std::vector<double> fallback) {
if (!n.getParam(key, fallback)) {
ROS_WARN_STREAM("[" << ros::this_node::getName() << "] Could not get string value for "
<< n.getNamespace() << "/" << key
std::vector<double> getParameter(const ros::NodeHandle& n, const std::string& key, std::vector<double> fallback)
{
if (!n.getParam(key, fallback))
{
ROS_WARN_STREAM("[" << ros::this_node::getName() << "] Could not get value for " << n.getNamespace() << "/" << key
<< " from param server, using default empty vector.");
}
return fallback;
}
template <>
std::vector<float> getParameter(const ros::NodeHandle &n, const std::string &key, std::vector<float> fallback) {
if (!n.getParam(key, fallback)) {
ROS_WARN_STREAM("[" << ros::this_node::getName() << "] Could not get string value for "
<< n.getNamespace() << "/" << key
std::vector<float> getParameter(const ros::NodeHandle& n, const std::string& key, std::vector<float> fallback)
{
if (!n.getParam(key, fallback))
{
ROS_WARN_STREAM("Could not get value for " << n.getNamespace() << "/" << key
<< " from param server, using default empty vector.");
}
return fallback;
}
template <>
std::vector<int> getParameter(const ros::NodeHandle &n, const std::string &key, std::vector<int> fallback) {
if (!n.getParam(key, fallback)) {
ROS_WARN_STREAM("[" << ros::this_node::getName() << "] Could not get string value for "
<< n.getNamespace() << "/" << key
std::vector<int> getParameter(const ros::NodeHandle& n, const std::string& key, std::vector<int> fallback)
{
if (!n.getParam(key, fallback))
{
ROS_WARN_STREAM("Could not get value for " << n.getNamespace() << "/" << key
<< " from param server, using default empty vector.");
}
return fallback;
}
template <>
std::vector<bool> getParameter(const ros::NodeHandle &n, const std::string &key, std::vector<bool> fallback) {
if (!n.getParam(key, fallback)) {
ROS_WARN_STREAM("[" << ros::this_node::getName() << "] Could not get string value for "
<< n.getNamespace() << "/" << key
std::vector<bool> getParameter(const ros::NodeHandle& n, const std::string& key, std::vector<bool> fallback)
{
if (!n.getParam(key, fallback))
{
ROS_WARN_STREAM("Could not get value for " << n.getNamespace() << "/" << key
<< " from param server, using default empty vector.");
}
return fallback;
}
template <>
std::map<std::string, std::string>
getParameter(const ros::NodeHandle &n, const std::string &key, std::map<std::string, std::string> fallback) {
if (!n.getParam(key, fallback)) {
ROS_WARN_STREAM("[" << ros::this_node::getName() << "] Could not get string value for "
<< n.getNamespace() << "/" << key
std::map<std::string, std::string> getParameter(const ros::NodeHandle& n, const std::string& key,
std::map<std::string, std::string> fallback)
{
if (!n.getParam(key, fallback))
{
ROS_WARN_STREAM("Could not get value for " << n.getNamespace() << "/" << key
<< " from param server, using default empty map.");
}
return fallback;
}
template <>
std::map<std::string, double>
getParameter(const ros::NodeHandle &n, const std::string &key, std::map<std::string, double> fallback) {
if (!n.getParam(key, fallback)) {
ROS_WARN_STREAM("[" << ros::this_node::getName() << "] Could not get string value for "
<< n.getNamespace() << "/" << key
std::map<std::string, double> getParameter(const ros::NodeHandle& n, const std::string& key,
std::map<std::string, double> fallback)
{
if (!n.getParam(key, fallback))
{
ROS_WARN_STREAM("Could not get value for " << n.getNamespace() << "/" << key
<< " from param server, using default empty map.");
}
return fallback;
}
template <>
std::map<std::string, float>
getParameter(const ros::NodeHandle &n, const std::string &key, std::map<std::string, float> fallback) {
if (!n.getParam(key, fallback)) {
ROS_WARN_STREAM("[" << ros::this_node::getName() << "] Could not get string value for "
<< n.getNamespace() << "/" << key
std::map<std::string, float> getParameter(const ros::NodeHandle& n, const std::string& key,
std::map<std::string, float> fallback)
{
if (!n.getParam(key, fallback))
{
ROS_WARN_STREAM("Could not get value for " << n.getNamespace() << "/" << key
<< " from param server, using default empty map.");
}
return fallback;
}
template <>
std::map<std::string, int>
getParameter(const ros::NodeHandle &n, const std::string &key, std::map<std::string, int> fallback) {
if (!n.getParam(key, fallback)) {
ROS_WARN_STREAM("[" << ros::this_node::getName() << "] Could not get string value for "
<< n.getNamespace() << "/" << key
std::map<std::string, int> getParameter(const ros::NodeHandle& n, const std::string& key,
std::map<std::string, int> fallback)
{
if (!n.getParam(key, fallback))
{
ROS_WARN_STREAM("Could not get value for " << n.getNamespace() << "/" << key
<< " from param server, using default empty map.");
}
return fallback;
}
template <>
std::map<std::string, bool>
getParameter(const ros::NodeHandle &n, const std::string &key, std::map<std::string, bool> fallback) {
if (!n.getParam(key, fallback)) {
ROS_WARN_STREAM("[" << ros::this_node::getName() << "] Could not get string value for "
<< n.getNamespace() << "/" << key
std::map<std::string, bool> getParameter(const ros::NodeHandle& n, const std::string& key,
std::map<std::string, bool> fallback)
{
if (!n.getParam(key, fallback))
{
ROS_WARN_STREAM("Could not get value for " << n.getNamespace() << "/" << key
<< " from param server, using default empty map.");
}
return fallback;
}
/** @endcond */
}
} // namespace CetiRosToolbox
#endif // CCF_NODEUTIL_H
//
// Created by sebastian on 14.01.21.
//
#ifndef CCF_SCENECOLLISIONOBJECT_H
#define CCF_SCENECOLLISIONOBJECT_H
#include <string>
#include <std_msgs/ColorRGBA.h>
#include <moveit_msgs/CollisionObject.h>
class SceneCollisionObject {
public:
SceneCollisionObject();
const moveit_msgs::CollisionObject &getCollisionObject() const;
void setCollisionObject(const moveit_msgs::CollisionObject &collisionObject);
const std::string &getType() const;
void setType(const std::string &type);
const std_msgs::ColorRGBA &getColor() const;
void setColor(const std_msgs::ColorRGBA &color);
private:
moveit_msgs::CollisionObject collisionObject;
std::string type;
std_msgs::ColorRGBA color;
};
#endif //CCF_SCENECOLLISIONOBJECT_H
......@@ -2,29 +2,62 @@
<arg name="robot_ip"/>
<arg name="load_gripper" default="true"/>
<arg name="rviz_config" default="$(find ccf)/config/config.rviz"/>
<include file="$(find franka_control)/launch/franka_control.launch" >
<!-- Launch real-robot control (replaced the contents of the following file) -->
<!-- <include file="$(find franka_control)/launch/franka_control.launch" pass_all_args="true"/> -->
<!-- beginning of content of $(find franka_control)/launch/franka_control.launch -->
<arg name="robot" default="panda" doc="choose your robot. Possible values: [panda, fr3]"/>
<arg name="arm_id" default="$(arg robot)" />
<arg name="xacro_args" default="" />
<param name="robot_description" command="$(find xacro)/xacro $(find franka_description)/robots/$(arg robot)/$(arg robot).urdf.xacro hand:=$(arg load_gripper) arm_id:=$(arg arm_id) $(arg xacro_args)"/>
<include file="$(find franka_gripper)/launch/franka_gripper.launch" if="$(arg load_gripper)">
<arg name="robot_ip" value="$(arg robot_ip)" />
<arg name="load_gripper" value="$(arg load_gripper)"/>
<arg name="arm_id" value="$(arg arm_id)" />
</include>
<!-- Load joint controller configurations from YAML file to parameter server -->
<rosparam file="$(find ccf)/config/panda_control.yaml" command="load" />
<node name="franka_control" pkg="franka_control" type="franka_control_node" output="screen" required="true">
<rosparam command="load" file="$(find franka_control)/config/franka_control_node.yaml" subst_value="true" />
<rosparam command="load" file="$(find ccf)/config/franka_control_node.yaml" subst_value="true" />
<param name="robot_ip" value="$(arg robot_ip)" />
</node>
<rosparam command="load" file="$(find franka_control)/config/default_controllers.yaml" subst_value="true" />
<node name="state_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="franka_state_controller"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen"/>
<node name="joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher" output="screen">
<rosparam if="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states, franka_gripper/joint_states] </rosparam>
<rosparam unless="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states] </rosparam>
<param name="rate" value="30"/>
</node>
<!-- end of content of $(find franka_control)/launch/franka_control.launch -->
<param name="move_group/trajectory_execution/allowed_execution_duration_scaling" value="4.0"/>
<param name="move_group/trajectory_execution/execution_duration_monitoring" value="false"/>
<include file="$(find panda_moveit_config)/launch/planning_context.launch">
<arg name="load_robot_description" value="true" />
<arg name="load_gripper" value="$(arg load_gripper)" />
</include>
<include file="$(find panda_moveit_config)/launch/move_group.launch">
<arg name="load_gripper" value="$(arg load_gripper)" />
<!-- By default use joint position controllers -->
<arg name="transmission" default="position"/>
<!-- Start ROS controllers -->
<include file="$(find panda_moveit_config)/launch/ros_controllers.launch" pass_all_args="true"/>
<!-- as well as MoveIt demo -->
<include file="$(find panda_moveit_config)/launch/demo.launch" pass_all_args="true">
<!-- robot description is loaded by franka_control.launch -->
<arg name="load_robot_description" value="false"/>
<!-- MoveItSimpleControllerManager provides ros_control's JointTrajectory controllers
as well as GripperCommand actions -->
<arg name="moveit_controller_manager" value="simple"/>
<arg name="use_rviz" value="false"/>
</include>
<node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_1" args="0 0 0 0 0 0 1 world panda_link0" />
<node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_1"
args="0 0 0 0 0 0 1 world panda_link0"/>
<include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
<include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" pass_all_args="true"/>
<rosparam file="$(find ccf)/config/config.yaml" command="load"/>
<rosparam file="$(find ccf)/config/config_realrobot.yaml" command="load"/>
......
<launch>
<!-- based on demo.launch from panda_moveit_config -->
<include file="$(find panda_simulation)/launch/simulation.launch"/>
<arg name="arm_id" default="panda" />
<!-- specify the planning pipeline -->
<arg name="pipeline" default="ompl" />
<!-- By default, we are not in debug mode -->
<arg name="debug" default="false" />
<!-- By default we will load the gripper -->
<arg name="load_gripper" default="true" />
<!-- By default, we will load or override the robot_description -->
<arg name="load_robot_description" default="true"/>
<!-- Choose controller manager: fake, simple, or ros_control -->
<arg name="moveit_controller_manager" default="fake" />
<!-- Set execution mode for fake execution controllers -->
<arg name="fake_execution_type" default="interpolate" />
<!-- Transmission used for joint control: position, velocity, or effort -->
<arg name="transmission" />
<!-- By default, hide joint_state_publisher's GUI in 'fake' controller_manager mode -->
<arg name="use_gui" default="false" />
<arg name="use_rviz" default="true" />
<!-- Use rviz config for MoveIt tutorial -->
<arg name="rviz_tutorial" default="false" />
<!-- If needed, broadcast static tf for robot root -->
<node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world $(arg arm_id)_link0" />
<group if="$(eval arg('moveit_controller_manager') == 'fake')">
<!-- We do not have a real robot connected, so publish fake joint states via a joint_state_publisher
MoveIt's fake controller's joint states are considered via the 'source_list' parameter -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)">
<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
</node>
<!-- If desired, a GUI version is available allowing to move the simulated robot around manually
This corresponds to moving around the real robot without the use of MoveIt. -->
<node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" if="$(arg use_gui)">
<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
</node>
<!-- Given the published joint states, publish tf for the robot links -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
</group>
<!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
<include file="$(find panda_moveit_config)/launch/move_group.launch" pass_all_args="true">
<arg name="allow_trajectory_execution" value="true" />
<arg name="info" value="true" />
</include>
<node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false"
args="-d $(find ccf)/config/config.rviz" output="screen">
args="-d $(find ccf)/config/config.rviz" output="screen" if="$(arg use_rviz)">
<rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
</node>
......
Subproject commit 64a5ff3c3b71fe019353aeacaebc66a3cf4f3461
Subproject commit 2ff3d155dcd10564f1816675789284b4efd79eb7
......@@ -33,7 +33,7 @@
<depend>moveit_ros_planning_interface</depend>
<build_depend>geometry_msgs</build_depend>
<depend>franka_gripper</depend>
<depend>libnng</depend>
<depend>paho_mqtt</depend>
<build_depend>tf2</build_depend>
<build_depend>protobuf</build_depend>
<build_depend>std_msgs</build_depend>
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment