Skip to content
GitLab
Commit 389567cf authored by Johannes Mey's avatar Johannes Mey
Browse files

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

parents 9a442a0d 32800166
Hide whitespace changes
Inline Side-by-side
.gitmodules deleted 100644 → 0
View file @ 9a442a0d
[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
CMakeLists.txt
View file @ 389567cf
......@@ -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}
)
#############
......
config/config.yaml
View file @ 389567cf
......@@ -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
config/franka_control_node.yaml 0 → 100644
View file @ 389567cf
# 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]
config/panda_control.yaml deleted 100644 → 0
View file @ 9a442a0d
# 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
include/ccf/connection/Connection.h
View file @ 389567cf
......@@ -9,29 +9,31 @@
#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;
/// callback defined by the initializeConnection() method
std::function<void(std::string, std::string)> messageReceivedCallback;
public:
/// Listen on a specific channel. By default, this method returns true. It must not be implemented, but then it is
/// 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; };
/// 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
/// \param channel
/// \param message
/// \return true if the message was sent successfully
virtual bool send(const std::string &channel, const std::string &message) = 0;
/// Listen on a specific channel. By default, this method returns true. It must not be implemented, but then it is
/// 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;
};
/// 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
/// \param channel
/// \param message
/// \return true if the message was sent successfully
virtual bool send(const std::string& channel, const std::string& message) = 0;
};
#endif //CCF_CONNECTION_H
#endif // CCF_CONNECTION_H
include/ccf/connection/MqttConnection.h
View file @ 389567cf
......@@ -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;
const int QOS = 0;
class MqttConnection : virtual public Connection
{
std::vector<std::string> topics;
const int QOS = 0;
mqtt::async_client cli_;
mqtt::connect_options connOpts_;
public:
MqttConnection(const std::string& mqttAddress, const std::string& client_id);
explicit 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;
~MqttConnection();
private:
bool send(const std::string& channel, const std::string& message) override;
void receiveMessages();
bool initializeConnection(std::function<void(std::string, std::string)> callback) override;
mqtt::client &getClient();
bool listen(const std::string& topic) override;
bool ensureConnection();
~MqttConnection();
};
#endif //CCF_MQTTCONNECTION_H
\ No newline at end of file
#endif // CCF_MQTTCONNECTION_H
include/ccf/connection/NngConnection.h
View file @ 389567cf
......@@ -13,44 +13,43 @@
/// 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 {
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:
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;
/// 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
/// \param server true, if the connection is a server (listens for connections), otherwise client mode is enabled
explicit NngConnection(const std::string &connection_address, bool server = true);
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
/// \param server true, if the connection is a server (listens for connections), otherwise client mode is enabled
explicit NngConnection(const std::string& connection_address, bool server = true);
bool send(const std::string &channel, const std::string &message) override;
bool send(const std::string& channel, const std::string& message) override;
/// NNG pair does not support topics, so when it receives a message, this topic is assumed
[[nodiscard]] const std::string &getReceiveTopic() const;
/// NNG pair does not support topics, so when it receives a message, this topic is assumed
[[nodiscard]] const std::string& getReceiveTopic() const;
/// NNG pair does not support topics, so when it sends a message, this topic is the only one that is actually sent
[[nodiscard]] const std::string &getSendTopic() const;
/// NNG pair does not support topics, so when it sends a message, this topic is the only one that is actually sent
[[nodiscard]] const std::string& getSendTopic() const;
/// set the topic that NNG actually sends, messages on other topics are ignored
/// \param newTopic
void setSendTopic(const std::string &newTopic);
/// set the topic that NNG actually sends, messages on other topics are ignored
/// \param newTopic
void setSendTopic(const std::string& newTopic);
/// set the topic NNG forwards received messages with
/// \param newTopic
void setReceiveTopic(const std::string &newTopic);
/// set the topic NNG forwards received messages with
/// \param newTopic
void setReceiveTopic(const std::string& newTopic);
bool initializeConnection(std::function<void(std::string, std::string)> callback) override;
bool initializeConnection(std::function<void(std::string, std::string)> callback) override;
void receiveMessages();
void receiveMessages();
~NngConnection();
~NngConnection();
};
#endif //CCF_NNGCONNECTION_H
#endif // CCF_NNGCONNECTION_H
include/ccf/connection/RosConnection.h
View file @ 389567cf
......@@ -10,53 +10,52 @@
#include <ros/ros.h>
#include <map>
class RosConnection : public Connection {
std::vector<std::string> ros_topics;
std::unique_ptr<std::thread> ros_spinner_thread;
std::vector<ros::Subscriber> ros_subscribers;
std::map<std::string, ros::Publisher> ros_publishers;
ros::NodeHandle node_handle;
int input_queue_size;
int output_queue_size;
class RosConnection : public Connection
{
std::vector<std::string> ros_topics;
std::unique_ptr<std::thread> ros_spinner_thread;
std::vector<ros::Subscriber> ros_subscribers;
std::map<std::string, ros::Publisher> ros_publishers;
ros::NodeHandle node_handle;
int input_queue_size;
int output_queue_size;
public:
/**
* A connection to the ROS based communication, limited to std_messages/String.
* @param n a valid node handle
* @param input_queue_size of the subscribers
* @param output_queue_size of the publishers
*/
explicit RosConnection(const ros::NodeHandle &n, int input_queue_size, int output_queue_size);
bool send(const std::string &channel, const std::string &message) override;
bool initializeConnection(std::function<void(std::string, std::string)> callback) override;
bool listen(const std::string &channel) override;
/**
* Grands the possibility to change the node handle and thus the sender namespace.
* @param n the node handel to be set
*/
void setHandle(const ros::NodeHandle &n);
/**
* 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.
* @param channel the topic on which the publisher sends
* @return true if successfull added
*/
bool addPublisher(const std::string &channel);
/**
* Receive messages on all configured topics.
*/
void receiveMessages();
~RosConnection();
/**
* A connection to the ROS based communication, limited to std_messages/String.
* @param n a valid node handle
* @param input_queue_size of the subscribers
* @param output_queue_size of the publishers
*/
explicit RosConnection(const ros::NodeHandle& n, int input_queue_size, int output_queue_size);
bool send(const std::string& channel, const std::string& message) override;
bool initializeConnection(std::function<void(std::string, std::string)> callback) override;
bool listen(const std::string& channel) override;
/**
* Grands the possibility to change the node handle and thus the sender namespace.
* @param n the node handel to be set
*/
void setHandle(const ros::NodeHandle& n);
/**
* 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.
* @param channel the topic on which the publisher sends
* @return true if successfull added
*/
bool addPublisher(const std::string& channel);
/**
* Receive messages on all configured topics.
*/
void receiveMessages();
~RosConnection();
};
#endif //SRC_ROSCONNECTION_H
#endif // SRC_ROSCONNECTION_H
include/ccf/controller/Controller.h
View file @ 389567cf
......@@ -12,29 +12,28 @@
#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;
std::vector<std::shared_ptr<Connection>> connections;
ros::NodeHandle node_handle_;
std::vector<std::shared_ptr<Connection>> connections;
void receive(const std::string &channel, const std::string &data);
void receive(const std::string& channel, const std::string& data);
public:
explicit Controller(const ros::NodeHandle &nodeHandle);
// common functionality
void addConnection(const std::shared_ptr<Connection> &connection);
explicit Controller(const ros::NodeHandle& nodeHandle);
void addCallback(const std::string &channel, const std::function<void(const std::string)> &function);
// common functionality
void addConnection(const std::shared_ptr<Connection>& connection);
void sendToAll(const std::string &channel, const std::string &message);
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
#endif // CCF_CONTROLLER_H
include/ccf/controller/DummyRobotArmController.h
View file @ 389567cf
......@@ -7,30 +7,38 @@
#include "RobotArmController.h"
class DummyRobotArmController : public virtual RobotArmController {
class DummyRobotArmController : public virtual RobotArmController
{
public:
explicit DummyRobotArmController(const ros::NodeHandle &nodeHandle, const std::string &cellName);
bool pickAndDrop(Object &robot, Object &object, Object &bin, bool simulateOnly) override;
bool pickAndPlace(Object &robot, Object &object, Object &location, bool simulateOnly) 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.
/// Otherwise, we assume that we can reach it.
/// \param robot
/// \param location
/// \param object to be picked and placed. The current location of this object is ignored.
/// \return true if the location is reachable
bool reachableLocation(const Object &robot, const Object &location, const Object &object) override;
/// Compute if an object is reachable by a robot, i.e., if an object can grasp it
/// \param robot
/// \param object to be picked
/// \return true if the location is reachable
bool reachableObject(const Object &robot, const Object &object) override;
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.
/// Otherwise, we assume that we can reach it.
/// \param robot
/// \param location
/// \param object to be picked and placed. The current location of this object is ignored.
/// \return true if the location is reachable
bool reachableLocation(const Object& robot, const Object& location, const Object& object) override;
/// Compute if an object is reachable by a robot, i.e., if an object can grasp it
/// \param robot
/// \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
#endif // CCF_DUMMYROBOTARMCONTROLLER_H
include/ccf/controller/MoveItRobotArmController.h
View file @ 389567cf
//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;
ros::ServiceServer get_scene_service;
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;
/**
* tf2 buffer to receive current robot state
*/