Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
CeTI
ROS
CeTI Cobotic Framework
CCF - The CeTI Cobotic Framework
Commits
389567cf
Commit
389567cf
authored
Dec 06, 2022
by
Johannes Mey
Browse files
Merge branch 'noetic/feature/robotics-festival' into noetic/main
parents
9a442a0d
32800166
Changes
31
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
ensure
Connection
();
~
Mqtt
Connection
();
};
#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
node
H
andle
;
std
::
vector
<
std
::
shared_ptr
<
Connection
>>
connections
;
ros
::
NodeHandle
node
_h
andle
_
;
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
*/