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

.gitmodules

-[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

@@ -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

@@ -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

+# 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

-# 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

@@ -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

include/ccf/connection/MqttConnection.h

@@ -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

include/ccf/connection/NngConnection.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

include/ccf/connection/RosConnection.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

include/ccf/controller/Controller.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

include/ccf/controller/DummyRobotArmController.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

include/ccf/controller/MoveItRobotArmController.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

include/ccf/controller/RobotArmController.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

include/ccf/util/NodeUtil.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

include/ccf/util/scene_collision_object.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

launch/robot_setup.launch

@@ -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/simulation_setup.launch

 <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>
 

paho.mqtt.c @ 64a5ff3c

-Subproject commit 64a5ff3c3b71fe019353aeacaebc66a3cf4f3461

paho.mqtt.cpp @ 2ff3d155

-Subproject commit 2ff3d155dcd10564f1816675789284b4efd79eb7

package.xml

@@ -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>