diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7a09237d75a274bc0d36e0afbb307deea3640b80..69b371c990bd02ea4040ae38c1b2a842f4aecfc7 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -43,8 +43,6 @@ message("psource " ${PROTO_SRC})
message("pheaders" ${PROTO_HDRS})
find_package(PkgConfig REQUIRED)
-pkg_check_modules(JSONCPP jsoncpp)
-message(${JSONCPP_LIBRARIES})
add_subdirectory(lib/paho.mqtt.c)
@@ -90,36 +88,32 @@ include_directories(
add_dependencies(paho-mqttpp3 paho-mqtt3c)
add_library(TrajectoryUtil src/util/TrajectoryUtil.cpp src/util/TrajectoryUtil.h)
+add_library(MqttUtil src/util/MqttUtil.cpp src/util/MqttUtil.h)
# Specify libraries to link a library or executable target against
target_link_libraries(TrajectoryUtil ${catkin_LIBRARIES})
-add_executable(robot_control_node src/robot_control_node.cpp)
-add_executable(SafetyAwarePlanner src/SafetyAwarePlanner.cpp src/SafetyEnvironmentCreator.cpp)
-add_executable(SampleConstraintPlanner src/samples/SampleConstraintPlanner.cpp)
-add_executable(SampleTimedCartesianPlanner src/samples/SampleTimedCartesianPlanner.cpp)
-add_executable(RobotStateProvider src/RobotStateProvider.cpp src/mem_persistence.cpp ${PROTO_SRCS} ${PROTO_HDRS})
+add_executable(RobotStateProvider src/RobotStateProvider.cpp ${PROTO_SRCS} ${PROTO_HDRS})
add_executable(TimedPlanner src/TimedPlanner.cpp)
add_executable(MqttToRosNode src/MqttToRosNode.cpp ${PROTO_SRCS} ${PROTO_HDRS})
add_executable(MqttRosConnectionTestNode src/MqttRosConnectionTestNode.cpp ${PROTO_SRCS} ${PROTO_HDRS})
-target_link_libraries(robot_control_node ${catkin_LIBRARIES} boost_filesystem ${JSONCPP_LIBRARIES})
-target_link_libraries(SafetyAwarePlanner ${catkin_LIBRARIES})
-target_link_libraries(SampleConstraintPlanner ${catkin_LIBRARIES})
-target_link_libraries(SampleTimedCartesianPlanner ${catkin_LIBRARIES})
target_link_libraries(RobotStateProvider LINK_PUBLIC
+ MqttUtil
${catkin_LIBRARIES}
paho-mqttpp3
${Protobuf_LIBRARIES}
)
target_link_libraries(MqttToRosNode LINK_PUBLIC
+ MqttUtil
${catkin_LIBRARIES}
paho-mqttpp3
${Protobuf_LIBRARIES}
)
target_link_libraries(MqttRosConnectionTestNode LINK_PUBLIC
+ MqttUtil
${catkin_LIBRARIES}
paho-mqttpp3
${Protobuf_LIBRARIES}
)
-target_link_libraries(TimedPlanner ${catkin_LIBRARIES} TrajectoryUtil)
\ No newline at end of file
+target_link_libraries(TimedPlanner ${catkin_LIBRARIES} TrajectoryUtil)
diff --git a/launch/simulation_rosrag.launch b/launch/simulation_rosrag.launch
index b662c1a7be4a3ab036a0a64b4f1252ece5cdf1a8..120adc7cb41fa1c6d96a99adb1c6ee0fc13cadb0 100644
--- a/launch/simulation_rosrag.launch
+++ b/launch/simulation_rosrag.launch
@@ -1,7 +1,5 @@
<launch>
<include file="$(find panda_simulation)/launch/simulation.launch"/>
- <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
- <node name="rqt_console" pkg="rqt_console" type="rqt_console" />
<node name="TimedPlanner" pkg="panda_mqtt_connector" type="TimedPlanner" respawn="false" output="screen"/>
<node name="MqttToRosNode" pkg="panda_mqtt_connector" type="MqttToRosNode" respawn="false" output="screen"/>
diff --git a/launch/simulation_rosrag_test.launch b/launch/simulation_rosrag_test.launch
index c1b2c2cbf92f960ab8405dc7ac7fb1dc11f1a3f5..c489c2db372a223e3d4e938978e71a821a40a3b0 100644
--- a/launch/simulation_rosrag_test.launch
+++ b/launch/simulation_rosrag_test.launch
@@ -1,7 +1,5 @@
<launch>
<include file="$(find panda_simulation)/launch/simulation.launch"/>
- <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
- <node name="rqt_console" pkg="rqt_console" type="rqt_console" />
<node name="TimedPlanner" pkg="panda_mqtt_connector" type="TimedPlanner" respawn="false" output="screen"/>
<node name="MqttToRosNode" pkg="panda_mqtt_connector" type="MqttToRosNode" respawn="false" output="screen"/>
diff --git a/launch/simulation_test_test.launch b/launch/simulation_test_test.launch
new file mode 100644
index 0000000000000000000000000000000000000000..69da94afc9379fe1a0357157523d5863a707e771
--- /dev/null
+++ b/launch/simulation_test_test.launch
@@ -0,0 +1,3 @@
+<launch>
+ <node name="MqttRosConnectionTestNode" pkg="panda_mqtt_connector" type="MqttRosConnectionTestNode" respawn="false" output="screen"/>
+</launch>
diff --git a/package.xml b/package.xml
index 8a2ba17707df0b9b8401228bcabed00090455024..7aa73dd2f1ba748fa553ffeefe129d4452321dbf 100644
--- a/package.xml
+++ b/package.xml
@@ -33,7 +33,6 @@
<depend>std_msgs</depend>
<depend>tf</depend>
<depend>xacro</depend>
- <depend>libjsoncpp-dev</depend>
<depend>moveit_simple_controller_manager</depend>
<!-- CUSTOM -->
@@ -48,6 +47,5 @@
<!-- The export tag contains other, unspecified, tags -->
<export>
- <controller_interface plugin="${prefix}/controller_plugins.xml" />
</export>
</package>
diff --git a/src/MqttRosConnectionTestNode.cpp b/src/MqttRosConnectionTestNode.cpp
index 25ee0a61819d0f665f57cf89af9a1b39d525690b..f5a8798976b4a5bd1d5acfa2f4c0ce247f0844d4 100644
--- a/src/MqttRosConnectionTestNode.cpp
+++ b/src/MqttRosConnectionTestNode.cpp
@@ -1,17 +1,18 @@
//
// Created by sebastian on 31.03.20.
//
+#include "dataconfig.pb.h"
+#include "robotconfig.pb.h"
+#include "trajectory.pb.h"
+
#include "ros/ros.h"
#include <gazebo_msgs/LinkStates.h>
+
+#include "util/MqttUtil.h"
+
#include <mqtt/client.h>
-#include "string.h"
-#include "robotconfig.pb.h"
-#include "dataconfig.pb.h"
-#include "mem_persistence.cpp"
-#include "trajectory.pb.h"
using namespace std;
-using namespace std::chrono;
/*
* mqtt-topics for all links
@@ -21,224 +22,126 @@ std::string topics[11] = {"panda_ground", "panda_link_0", "panda_link_1", "panda
"panda_link_3", "panda_link_4", "panda_link_5", "panda_link_6",
"panda_link_7", "panda_link_8", "panda_link_9"};
-const string SERVER_ADDRESS { "tcp://localhost:1883" };
-const string CLIENT_ID { "ros_mqtt_tester" };
-
-bool mqttSetup = false;
-bool isInitialized = false;
-
-const int QOS = 1;
-
-mem_persistence persist;
-mqtt::client client("tcp://localhost:1883", "robot_state_provider_mqtt_test", &persist);
+const string CLIENT_ID{"ros_mqtt_tester"};
-void setupMqtt() {
+MqttUtil mqttUtil(CLIENT_ID);
- if (!mqttSetup) {
- std::cout << "MQTT: Initializing." << std::endl;
+void testTrajectoryUpdate(ros::NodeHandle n) {
- mqtt::connect_options connOpts;
- connOpts.set_keep_alive_interval(20);
- connOpts.set_clean_session(true);
- std::cout << "MQTT: Initialized." << std::endl;
+ ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", ">>>>>>> STARTING TRAJECTORY UPDATE TEST <<<<<<<");
- std::cout << "MQTT: Connecting." << std::endl;
- client.connect(connOpts);
- std::cout << "MQTT: Connected." << std::endl;
- mqttSetup = true;
- }
-}
-
-bool try_reconnect(mqtt::client& cli)
-{
- constexpr int N_ATTEMPT = 30;
-
- for (int i=0; i<N_ATTEMPT && !cli.is_connected(); ++i) {
- try {
- cli.reconnect();
- return true;
- }
- catch (const mqtt::exception&) {
- this_thread::sleep_for(seconds(1));
- }
- }
- return false;
-}
-
-void testTrajectoryUpdate(ros::NodeHandle n){
+ plan::Trajectory traj;
- std::cout << ">>>>>>>>>>>> STARTING TRAJECTORY UPDATE TEST <<<<<<<<<<<<<" << std::endl;
- setupMqtt();
+ plan::Trajectory_Position pos1;
+ pos1.set_x(0.6);
+ pos1.set_y(0.0);
+ pos1.set_z(0.6);
- plan::Trajectory traj;
+ traj.add_pos()->CopyFrom(pos1);
- plan::Trajectory_Position pos1;
- pos1.set_x(0.6);
- pos1.set_y(0.0);
- pos1.set_z(0.6);
+ plan::Trajectory_Position pos2;
+ pos2.set_x(-0.6);
+ pos2.set_y(0.0);
+ pos2.set_z(0.6);
- traj.add_pos()->CopyFrom(pos1);
+ traj.add_pos()->CopyFrom(pos2);
- plan::Trajectory_Position pos2;
- pos2.set_x(-0.6);
- pos2.set_y(0.0);
- pos2.set_z(0.6);
+ std::string mqtt_msg;
+ traj.SerializeToString(&mqtt_msg);
- traj.add_pos()->CopyFrom(pos2);
+ auto pubmsg = mqtt::make_message("trajectoryconfig", mqtt_msg);
+ mqttUtil.getClient().publish(pubmsg);
- std::string mqtt_msg;
- traj.SerializeToString(&mqtt_msg);
+ ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", ">>>>>>>>>>>> CHECKING SERVER <<<<<<<<<<<<<");
- auto pubmsg = mqtt::make_message("trajectoryconfig", mqtt_msg);
- pubmsg->set_qos(QOS);
- client.publish(pubmsg);
+ std::vector<double> x_pos;
+ std::vector<double> y_pos;
+ std::vector<double> z_pos;
- std::cout << ">>>>>>>>>>>> CHECKING SERVER <<<<<<<<<<<<<" << std::endl;
+ ros::Duration(5.0).sleep();
- std::vector<double> x_pos;
- std::vector<double> y_pos;
- std::vector<double> z_pos;
+ ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", "x configured: " << n.getParam("trajectory_pos_x", x_pos));
+ ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", "y configured: " << n.getParam("trajectory_pos_y", y_pos));
+ ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", "z configured: " << n.getParam("trajectory_pos_z", z_pos));
- ros::Duration(5.0).sleep();
-
- std::cout << "x configured: " << n.getParam("trajectory_pos_x", x_pos) << std::endl;
- std::cout << "y configured: " << n.getParam("trajectory_pos_y", y_pos) << std::endl;
- std::cout << "z configured: " << n.getParam("trajectory_pos_z", z_pos) << std::endl;
-
- for(int i = 0; i < x_pos.size(); i++)
- {
- std::cout << "X POS: " << x_pos.at(i) << std::endl;
- std::cout << "Y POS: " << y_pos.at(i) << std::endl;
- std::cout << "Z POS: " << z_pos.at(i) << std::endl;
- }
+ for (int i = 0; i < x_pos.size(); i++) {
+ ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", "X POS: " << x_pos.at(i));
+ ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", "Y POS: " << y_pos.at(i));
+ ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", "Z POS: " << z_pos.at(i));
+ }
}
-void testPlanningModeChange(ros::NodeHandle n){
+void testPlanningModeChange(ros::NodeHandle n) {
- std::cout << ">>>>>>>>>>>> STARTING PLANNING TEST <<<<<<<<<<<<<" << std::endl;
- setupMqtt();
+ ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", ">>>>>>> STARTING PLANNING MODE CHANGE TEST <<<<<<<");
- config::RobotConfig rc;
+ config::RobotConfig rc;
- rc.set_speed(1.0);
- rc.set_planningmode(config::RobotConfig_PlanningMode_FLUID);
+ rc.set_speed(1.0);
+ rc.set_planningmode(config::RobotConfig_PlanningMode_FLUID);
- std::string mqtt_msg;
- rc.SerializeToString(&mqtt_msg);
+ std::string mqtt_msg;
+ rc.SerializeToString(&mqtt_msg);
- auto pubmsg = mqtt::make_message("robotconfig", mqtt_msg);
- pubmsg->set_qos(QOS);
- client.publish(pubmsg);
+ auto pubmsg = mqtt::make_message("robotconfig", mqtt_msg);
+ mqttUtil.getClient().publish(pubmsg);
}
void testConfig(ros::NodeHandle n) {
- std::cout << ">>>>>>>>>>>> STARTING CONFIG TEST <<<<<<<<<<<<<" << std::endl;
- setupMqtt();
+ ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", ">>>>>>> STARTING CONFIG UPDATE TEST <<<<<<<");
- config::DataConfig dc;
+ config::DataConfig dc;
- dc.set_enableposition(true);
- dc.set_enableorientation(true);
- dc.set_enabletwistangular(true);
- dc.set_enabletwistlinear(true);
- dc.set_publishrate(1000);
+ dc.set_enableposition(true);
+ dc.set_enableorientation(true);
+ dc.set_enabletwistangular(true);
+ dc.set_enabletwistlinear(true);
+ dc.set_publishrate(1000);
- std::string mqtt_msg;
- dc.SerializeToString(&mqtt_msg);
+ std::string mqtt_msg;
+ dc.SerializeToString(&mqtt_msg);
- auto pubmsg = mqtt::make_message("dataconfig", mqtt_msg);
- pubmsg->set_qos(QOS);
- client.publish(pubmsg);
+ auto pubmsg = mqtt::make_message("dataconfig", mqtt_msg);
+ mqttUtil.getClient().publish(pubmsg);
}
-void receiveMqttMessages(ros::NodeHandle n)
-{
- mqtt::connect_options connOpts;
- mqtt::client cli(SERVER_ADDRESS, CLIENT_ID);
-
- connOpts.set_keep_alive_interval(20);
- connOpts.set_clean_session(true);
- try {
- ROS_INFO_STREAM("TEST Connecting to the MQTT server.");
- mqtt::connect_response rsp = cli.connect(connOpts);
- ROS_INFO_STREAM("TEST Connected to the MQTT server.");
-
- if (!rsp.is_session_present()) {
- ROS_INFO_STREAM("TEST Subscribing to topics.");
- cli.subscribe("configtest", 1);
- cli.subscribe("modetest", 1);
- cli.subscribe("trajectorytest", 1);
- ROS_INFO_STREAM("TEST Subscribed to topics.");
- }else {
- ROS_WARN_STREAM("TEST Session already present. Skipping subscribe.");
- }
-
- // Consume messages
- while (true) {
-
- auto msg = cli.consume_message();
-
- if (!msg) {
- if (!cli.is_connected()) {
- ROS_ERROR_STREAM("TEST Lost connection. Attempting reconnect");
- if (try_reconnect(cli)) {
- cli.subscribe("nodetest", 1);
- ROS_INFO_STREAM("TEST Reconnected");
- continue;
- }
- else {
- ROS_ERROR_STREAM("TEST Reconnect failed.");
- }
- }
- else {
- ROS_ERROR_STREAM("TEST An error occurred retrieving messages.");
- }
- break;
- }
-
- if (msg->get_topic() == "configtest")
- {
- testConfig(n);
- }
-
- if (msg->get_topic() == "modetest")
- {
- testPlanningModeChange(n);
- }
-
- if (msg->get_topic() == "trajectorytest")
- {
- testTrajectoryUpdate(n);
- }
- }
- }
- catch (const mqtt::exception& exc) {
- ROS_ERROR_STREAM("TEST: " << exc.what());
- cli.disconnect();
- return;
+void receiveMqttMessages(ros::NodeHandle n) {
+
+ if (mqttUtil.ensureConnection()) {
+ ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", "Waiting 500ms for message...");
+ mqtt::const_message_ptr *msg = new mqtt::const_message_ptr;
+ if (mqttUtil.getClient().try_consume_message_for(msg, std::chrono::milliseconds (500))) {
+ if (msg->get()->get_topic() == "test_config") {
+ testConfig(n);
+ } else if (msg->get()->get_topic() == "test_mode_change") {
+ testPlanningModeChange(n);
+ } else if (msg->get()->get_topic() == "test_trajectory_update") {
+ testTrajectoryUpdate(n);
+ }
}
- cli.disconnect();
+ } else {
+ ROS_ERROR_STREAM_NAMED("MqttRosConnectionTestNode", "Not connected! Unable to listen to messages.");
+ }
}
int main(int argc, char **argv) {
- ros::init(argc, argv, "robot_state_provider_test");
- ros::NodeHandle n;
- n.getParam("ready", isInitialized);
+ ros::init(argc, argv, "MqttRosConnectionTestNode");
+ ros::NodeHandle n;
- while (ros::ok()) {
+ ROS_INFO_NAMED("MqttRosConnectionTestNode", "Setting up MQTT.");
- // make sure the robot is initialized
- while (!isInitialized) {
- n.getParam("tud_planner_ready", isInitialized);
- }
+ mqttUtil.addTopic("test_config");
+ mqttUtil.addTopic("test_mode_change");
+ mqttUtil.addTopic("test_trajectory_update");
- receiveMqttMessages(n);
- ros::spin();
- }
+ mqttUtil.connect();
+
+ while (ros::ok()) {
+ receiveMqttMessages(n);
+ ros::spinOnce();
+ }
- // disconnect from mqtt
- client.disconnect();
- return 0;
+ return 0;
}
\ No newline at end of file
diff --git a/src/MqttToRosNode.cpp b/src/MqttToRosNode.cpp
index 8bd463c607d0b0de1862e1c1e02aaffab134d339..01e47bd70cca3d756aadd5e3df479f62b207d268 100644
--- a/src/MqttToRosNode.cpp
+++ b/src/MqttToRosNode.cpp
@@ -1,208 +1,135 @@
+#include "mqtt/client.h"
+#include <chrono>
#include <iostream>
#include <string>
-#include <cstring>
#include <thread>
-#include <chrono>
-#include "mqtt/client.h"
#include "ros/ros.h"
-#include "robotconfig.pb.h"
+#include "util/MqttUtil.h"
+
#include "dataconfig.pb.h"
+#include "robotconfig.pb.h"
#include "trajectory.pb.h"
using namespace std;
using namespace std::chrono;
-const string SERVER_ADDRESS { "tcp://localhost:1883" };
-const string CLIENT_ID { "ros_mqtt_consumer" };
-
-// Simple function to manually reconnect a client.
-bool try_reconnect(mqtt::client& cli)
-{
- constexpr int N_ATTEMPT = 30;
-
- for (int i=0; i<N_ATTEMPT && !cli.is_connected(); ++i) {
- try {
- cli.reconnect();
- return true;
- }
- catch (const mqtt::exception&) {
- this_thread::sleep_for(seconds(1));
- }
- }
- return false;
-}
+const string CLIENT_ID{"mqtt_to_ros"};
-void handleRobotConfig(config::RobotConfig robotConfig, ros::NodeHandle n)
-{
- std::cout << "Retrieved new target-speed: " << robotConfig.speed() << std::endl;
- n.setParam("robot_speed_factor", robotConfig.speed());
-
- std::cout << "Retrieved new loop-mode: " << robotConfig.looptrajectory() << std::endl;
- n.setParam("loop_trajectory", robotConfig.looptrajectory());
-
- std::cout << "Retrieved new planning-mode: " << std::endl;
- switch (robotConfig.planningmode())
- {
- case config::RobotConfig_PlanningMode_FLUID:
- n.setParam("robot_planning_mode", "fluid_path");
- std::cout << "Planning-mode: fluid" << std::endl;
- break;
- case config::RobotConfig_PlanningMode_CARTESIAN:
- n.setParam("robot_planning_mode", "cartesian_path");
- std::cout << "Planning-mode: cartesian" << std::endl;
- break;
- }
+MqttUtil mqttUtil(CLIENT_ID);
+
+void handleRobotConfig(config::RobotConfig robotConfig, ros::NodeHandle n) {
+ std::cout << "Retrieved new target-speed: " << robotConfig.speed() << std::endl;
+ n.setParam("robot_speed_factor", robotConfig.speed());
+
+ std::cout << "Retrieved new loop-mode: " << robotConfig.looptrajectory() << std::endl;
+ n.setParam("loop_trajectory", robotConfig.looptrajectory());
+
+ std::cout << "Retrieved new planning-mode: " << std::endl;
+ switch (robotConfig.planningmode()) {
+ case config::RobotConfig_PlanningMode_FLUID:
+ n.setParam("robot_planning_mode", "fluid_path");
+ std::cout << "Planning-mode: fluid" << std::endl;
+ break;
+ case config::RobotConfig_PlanningMode_CARTESIAN:
+ n.setParam("robot_planning_mode", "cartesian_path");
+ std::cout << "Planning-mode: cartesian" << std::endl;
+ break;
+ }
}
-void handleNewTrajectory(plan::Trajectory trajectory, ros::NodeHandle n)
-{
- ROS_INFO("Received new trajectory");
-
- /*XmlRpc::XmlRpcValue::ValueArray x_rpc_array;
- XmlRpc::XmlRpcValue::ValueArray y_rpc_array;
- XmlRpc::XmlRpcValue::ValueArray z_rpc_array;
-
- // build xml-rpc-lists for the parameter server
- for(int i = 0; i < trajectory.pos().size(); i++)
- {
- x_rpc_array.push_back(XmlRpc::XmlRpcValue(trajectory.pos().Get(i).x()));
- y_rpc_array.push_back(XmlRpc::XmlRpcValue(trajectory.pos().Get(i).y()));
- z_rpc_array.push_back(XmlRpc::XmlRpcValue(trajectory.pos().Get(i).z()));
- }*/
-
- std::vector<double> x_values;
- std::vector<double> y_values;
- std::vector<double> z_values;
-
- for(int i = 0; i < trajectory.pos().size(); i++)
- {
- x_values.push_back(trajectory.pos().Get(i).x());
- y_values.push_back(trajectory.pos().Get(i).y());
- z_values.push_back(trajectory.pos().Get(i).z());
- }
+void handleNewTrajectory(plan::Trajectory trajectory, ros::NodeHandle n) {
+ ROS_INFO("Received new trajectory");
+
+ std::vector<double> x_values;
+ std::vector<double> y_values;
+ std::vector<double> z_values;
- n.deleteParam("trajectory_pos_x");
- n.deleteParam("trajectory_pos_y");
- n.deleteParam("trajectory_pos_z");
+ for (int i = 0; i < trajectory.pos().size(); i++) {
+ x_values.push_back(trajectory.pos().Get(i).x());
+ y_values.push_back(trajectory.pos().Get(i).y());
+ z_values.push_back(trajectory.pos().Get(i).z());
+ }
- n.setParam("trajectory_pos_x", x_values);
- n.setParam("trajectory_pos_y", y_values);
- n.setParam("trajectory_pos_z", z_values);
+ n.deleteParam("trajectory_pos_x");
+ n.deleteParam("trajectory_pos_y");
+ n.deleteParam("trajectory_pos_z");
- n.setParam("new_trajectory_available", true);
+ n.setParam("trajectory_pos_x", x_values);
+ n.setParam("trajectory_pos_y", y_values);
+ n.setParam("trajectory_pos_z", z_values);
+
+ n.setParam("new_trajectory_available", true);
}
-void handleDataConfig(config::DataConfig dataConfig, ros::NodeHandle n)
-{
- std::cout << "Retrieved new data-config: -- publish-rate: " << dataConfig.publishrate()
- << " -- enablePosition: " << dataConfig.enableposition()
- << " -- enableOrientation: " << dataConfig.enableorientation()
- << " -- enableTwistLinear: " << dataConfig.enabletwistlinear()
- << " -- enableTwistAngular: " << dataConfig.enabletwistangular()
- << std::endl;
-
- n.setParam("data_publish_rate", dataConfig.publishrate());
- n.setParam("data_enable_position", dataConfig.enableposition());
- n.setParam("data_enable_orientation", dataConfig.enableorientation());
- n.setParam("data_enable_twist_linear", dataConfig.enabletwistlinear());
- n.setParam("data_enable_twist_angular", dataConfig.enabletwistangular());
+void handleDataConfig(config::DataConfig dataConfig, ros::NodeHandle n) {
+ std::cout << "Retrieved new data-config: -- publish-rate: " << dataConfig.publishrate()
+ << " -- enablePosition: " << dataConfig.enableposition()
+ << " -- enableOrientation: " << dataConfig.enableorientation()
+ << " -- enableTwistLinear: " << dataConfig.enabletwistlinear()
+ << " -- enableTwistAngular: " << dataConfig.enabletwistangular() << std::endl;
+
+ n.setParam("data_publish_rate", dataConfig.publishrate());
+ n.setParam("data_enable_position", dataConfig.enableposition());
+ n.setParam("data_enable_orientation", dataConfig.enableorientation());
+ n.setParam("data_enable_twist_linear", dataConfig.enabletwistlinear());
+ n.setParam("data_enable_twist_angular", dataConfig.enabletwistangular());
}
void handleMessage(const ros::NodeHandle &n, const mqtt::const_message_ptr &msg) {
- if (msg->get_topic() == "robotconfig")
- {
- const string rc_payload = msg->get_payload_str();
- config::RobotConfig robotConfig;
- robotConfig.ParseFromString(rc_payload);
- handleRobotConfig(robotConfig, n);
- }
-
- if (msg->get_topic() == "dataconfig")
- {
- const string dc_payload = msg->get_payload_str();
- config::DataConfig dataConfig;
- dataConfig.ParseFromString(dc_payload);
- handleDataConfig(dataConfig, n);
- }
-
- if (msg->get_topic() == "trajectoryconfig")
- {
- const string tc_payload = msg->get_payload_str();
- plan::Trajectory trajectoryConfig;
- trajectoryConfig.ParseFromString(tc_payload);
- handleNewTrajectory(trajectoryConfig, n);
- }
+ if (msg->get_topic() == "robotconfig") {
+ const string rc_payload = msg->get_payload_str();
+ config::RobotConfig robotConfig;
+ robotConfig.ParseFromString(rc_payload);
+ handleRobotConfig(robotConfig, n);
+ }
+
+ if (msg->get_topic() == "dataconfig") {
+ const string dc_payload = msg->get_payload_str();
+ config::DataConfig dataConfig;
+ dataConfig.ParseFromString(dc_payload);
+ handleDataConfig(dataConfig, n);
+ }
+
+ if (msg->get_topic() == "trajectoryconfig") {
+ const string tc_payload = msg->get_payload_str();
+ plan::Trajectory trajectoryConfig;
+ trajectoryConfig.ParseFromString(tc_payload);
+ handleNewTrajectory(trajectoryConfig, n);
+ }
}
-void receiveMqttMessages(ros::NodeHandle n)
-{
- mqtt::connect_options connOpts;
- mqtt::client cli(SERVER_ADDRESS, CLIENT_ID);
-
- connOpts.set_keep_alive_interval(20);
- connOpts.set_clean_session(true);
-
- const vector<string> TOPICS { "robotconfig", "dataconfig", "trajectoryconfig" };
- const vector<int> QOS { 1, 1, 1 };
-
- try {
- ROS_INFO_STREAM("Connecting to the MQTT server.");
- mqtt::connect_response rsp = cli.connect(connOpts);
- ROS_INFO_STREAM("Connected to the MQTT server.");
-
- if (!rsp.is_session_present()) {
- ROS_INFO_STREAM("Subscribing to topics.");
- cli.subscribe(TOPICS, QOS);
- ROS_INFO_STREAM("Subscribed to topics.");
- }else {
- ROS_WARN_STREAM("Session already present. Skipping subscribe.");
- }
-
- // Consume messages
- while (true) {
-
- auto msg = cli.consume_message();
-
- if (!msg) {
- if (!cli.is_connected()) {
- ROS_ERROR_STREAM("Lost connection. Attempting reconnect");
- if (try_reconnect(cli)) {
- cli.subscribe(TOPICS, QOS);
- ROS_INFO_STREAM("Reconnected");
- continue;
- }
- else {
- ROS_ERROR_STREAM("Reconnect failed.");
- }
- }
- else {
- ROS_ERROR_STREAM("An error occurred retrieving messages.");
- }
- break;
- }
-
- handleMessage(n, msg);
- //ROS_INFO_STREAM("NEW MESSAGE: " << msg->get_topic() << ": " << msg->to_string());
- }
+void receiveMqttMessages(ros::NodeHandle n) {
+ if (mqttUtil.ensureConnection()) {
+ mqtt::const_message_ptr msg;
+ if (mqttUtil.getClient().try_consume_message_for(&msg, std::chrono::milliseconds(500))) {
+ ROS_INFO_STREAM("NEW MESSAGE: " << msg.get()->get_topic() << ": " << msg.get()->to_string());
+ handleMessage(n, msg);
}
- catch (const mqtt::exception& exc) {
- ROS_ERROR_STREAM("MQTT2ROS " << exc.what());
- cli.disconnect();
- return;
- }
- cli.disconnect();
+ } else {
+ ROS_ERROR_STREAM_NAMED("MqttToRosNode", "Not connected! Unable to listen to messages.");
+ }
}
-int main(int argc, char* argv[])
-{
- ros::init(argc, argv, "mqtt_listener");
- ros::NodeHandle n;
+int main(int argc, char *argv[]) {
+ ros::init(argc, argv, "mqtt_listener");
+ ros::NodeHandle n;
+
+ mqttUtil.addTopic("robotconfig");
+ mqttUtil.addTopic("dataconfig");
+ mqttUtil.addTopic("trajectoryconfig");
+
+ if (!mqttUtil.connect()) {
+ ROS_INFO_STREAM("Unable to connect to MQTT server. Exiting...");
+ return -1;
+ }
+ while (ros::ok()) {
receiveMqttMessages(n);
+ ros::spinOnce();
+ }
- ros::spin();
- return 0;
+ return 0;
}
\ No newline at end of file
diff --git a/src/RobotStateProvider.cpp b/src/RobotStateProvider.cpp
index 82cbe76d1b5fc2e4c38f20b6f893e2504211940d..228119a6cb6aee7d5091bb7dbbe4114ba96e7b63 100644
--- a/src/RobotStateProvider.cpp
+++ b/src/RobotStateProvider.cpp
@@ -4,9 +4,10 @@
#include "ros/ros.h"
#include <gazebo_msgs/LinkStates.h>
#include <mqtt/client.h>
-#include "string.h"
+
#include "linkstate.pb.h"
-#include "mem_persistence.cpp"
+
+#include "util/MqttUtil.h"
bool export_to_log = false;
@@ -18,227 +19,198 @@ std::string topics[11] = {"panda_ground", "panda_link_0", "panda_link_1", "panda
"panda_link_3", "panda_link_4", "panda_link_5", "panda_link_6",
"panda_link_7", "panda_link_8", "panda_link_9"};
-bool mqttSetup = false;
bool isInitialized = false;
const int QOS = 1;
int current_redirect = 0;
-mem_persistence persist;
-mqtt::client client("tcp://localhost:1883", "robot_state_provider_mqtt", &persist);
+const std::string CLIENT_ID{"robot_state_provider_mqtt"};
+
+MqttUtil mqttUtil(CLIENT_ID);
/*
* logs to its own file in /.ros/logs (configured in launch-file)
*/
void exportMessageToLog(const gazebo_msgs::LinkStates &msg) {
- bool export_position = false;
- bool export_orientation = false;
- bool export_twist_linear = false;
- bool export_twist_angular = false;
+ bool export_position = false;
+ bool export_orientation = false;
+ bool export_twist_linear = false;
+ bool export_twist_angular = false;
- ros::NodeHandle n;
+ ros::NodeHandle n;
- n.getParam("data_enable_position", export_position);
- n.getParam("data_enable_orientation", export_orientation);
- n.getParam("data_enable_twist_linear", export_twist_linear);
- n.getParam("data_enable_twist_angular", export_twist_angular);
+ n.getParam("data_enable_position", export_position);
+ n.getParam("data_enable_orientation", export_orientation);
+ n.getParam("data_enable_twist_linear", export_twist_linear);
+ n.getParam("data_enable_twist_angular", export_twist_angular);
- ROS_INFO_STREAM("<<< LINK NAMES >>>");
+ ROS_INFO_STREAM("<<< LINK NAMES >>>");
- for (int i = 0; i < msg.name.size(); i++) {
- ROS_INFO_STREAM("[" << i << "] " << msg.name.at(i));
- }
+ for (int i = 0; i < msg.name.size(); i++) {
+ ROS_INFO_STREAM("[" << i << "] " << msg.name.at(i));
+ }
- if (export_position) {
+ if (export_position) {
- ROS_INFO_STREAM("<<< POSITIONS BY LINK >>>");
+ ROS_INFO_STREAM("<<< POSITIONS BY LINK >>>");
- for (int i = 0; i < msg.pose.size(); i++) {
- ROS_INFO_STREAM("[" << i << "] [x] " << msg.pose.at(i).position.x);
- ROS_INFO_STREAM("[" << i << "] [y] " << msg.pose.at(i).position.y);
- ROS_INFO_STREAM("[" << i << "] [z] " << msg.pose.at(i).position.z);
- }
+ for (int i = 0; i < msg.pose.size(); i++) {
+ ROS_INFO_STREAM("[" << i << "] [x] " << msg.pose.at(i).position.x);
+ ROS_INFO_STREAM("[" << i << "] [y] " << msg.pose.at(i).position.y);
+ ROS_INFO_STREAM("[" << i << "] [z] " << msg.pose.at(i).position.z);
}
+ }
- if (export_orientation) {
+ if (export_orientation) {
- ROS_INFO_STREAM("<<< ORIENTATIONS BY LINK >>>");
+ ROS_INFO_STREAM("<<< ORIENTATIONS BY LINK >>>");
- for (int i = 0; i < msg.pose.size(); i++) {
- ROS_INFO_STREAM("[" << i << "] [w] " << msg.pose.at(i).orientation.w);
- ROS_INFO_STREAM("[" << i << "] [x] " << msg.pose.at(i).orientation.x);
- ROS_INFO_STREAM("[" << i << "] [y] " << msg.pose.at(i).orientation.y);
- ROS_INFO_STREAM("[" << i << "] [z] " << msg.pose.at(i).orientation.z);
- }
+ for (int i = 0; i < msg.pose.size(); i++) {
+ ROS_INFO_STREAM("[" << i << "] [w] " << msg.pose.at(i).orientation.w);
+ ROS_INFO_STREAM("[" << i << "] [x] " << msg.pose.at(i).orientation.x);
+ ROS_INFO_STREAM("[" << i << "] [y] " << msg.pose.at(i).orientation.y);
+ ROS_INFO_STREAM("[" << i << "] [z] " << msg.pose.at(i).orientation.z);
}
-
- if (export_twist_linear) {
- ROS_INFO_STREAM("<<< LINEAR TWISTS BY LINK >>>");
- for (int i = 0; i < msg.twist.size(); i++) {
- ROS_INFO_STREAM("[" << i << "] [x] " << msg.twist.at(i).linear.x);
- ROS_INFO_STREAM("[" << i << "] [y] " << msg.twist.at(i).linear.y);
- ROS_INFO_STREAM("[" << i << "] [z] " << msg.twist.at(i).linear.z);
- }
+ }
+
+ if (export_twist_linear) {
+ ROS_INFO_STREAM("<<< LINEAR TWISTS BY LINK >>>");
+ for (int i = 0; i < msg.twist.size(); i++) {
+ ROS_INFO_STREAM("[" << i << "] [x] " << msg.twist.at(i).linear.x);
+ ROS_INFO_STREAM("[" << i << "] [y] " << msg.twist.at(i).linear.y);
+ ROS_INFO_STREAM("[" << i << "] [z] " << msg.twist.at(i).linear.z);
}
-
- if (export_twist_angular) {
- ROS_INFO_STREAM("<<< ANGULAR TWISTS BY LINK >>>");
- for (int i = 0; i < msg.twist.size(); i++) {
- ROS_INFO_STREAM("[" << i << "] [x] " << msg.twist.at(i).angular.x);
- ROS_INFO_STREAM("[" << i << "] [y] " << msg.twist.at(i).angular.y);
- ROS_INFO_STREAM("[" << i << "] [z] " << msg.twist.at(i).angular.z);
- }
+ }
+
+ if (export_twist_angular) {
+ ROS_INFO_STREAM("<<< ANGULAR TWISTS BY LINK >>>");
+ for (int i = 0; i < msg.twist.size(); i++) {
+ ROS_INFO_STREAM("[" << i << "] [x] " << msg.twist.at(i).angular.x);
+ ROS_INFO_STREAM("[" << i << "] [y] " << msg.twist.at(i).angular.y);
+ ROS_INFO_STREAM("[" << i << "] [z] " << msg.twist.at(i).angular.z);
}
+ }
}
panda::PandaLinkState buildLinkStateMessage(const gazebo_msgs::LinkStates &msg, int link_number) {
- bool export_position = false;
- bool export_orientation = false;
- bool export_twist_linear = false;
- bool export_twist_angular = false;
-
- ros::NodeHandle n;
-
- n.getParam("data_enable_position", export_position);
- n.getParam("data_enable_orientation", export_orientation);
- n.getParam("data_enable_twist_linear", export_twist_linear);
- n.getParam("data_enable_twist_angular", export_twist_angular);
-
- //std::cout << "Building LinkStateMessage with pos: " << export_position << " ori: " << export_orientation << " twistlin: "
- // << export_twist_linear << " twistang: " << export_twist_angular << std::endl;
-
- panda::PandaLinkState pls_msg;
-
- pls_msg.set_name(msg.name.at(link_number));
-
- if (export_position) {
- panda::PandaLinkState_Position *pos = new panda::PandaLinkState_Position();
- pos->set_positionx(msg.pose.at(link_number).position.x);
- pos->set_positiony(msg.pose.at(link_number).position.y);
- pos->set_positionz(msg.pose.at(link_number).position.z);
- pls_msg.set_allocated_pos(pos);
- }
-
- if (export_orientation) {
- panda::PandaLinkState_Orientation *orient = new panda::PandaLinkState_Orientation();
- orient->set_orientationw(msg.pose.at(link_number).orientation.w);
- orient->set_orientationx(msg.pose.at(link_number).orientation.x);
- orient->set_orientationy(msg.pose.at(link_number).orientation.y);
- orient->set_orientationz(msg.pose.at(link_number).orientation.z);
- pls_msg.set_allocated_orient(orient);
- }
-
- if (export_twist_linear) {
- panda::PandaLinkState_TwistLinear *tl = new panda::PandaLinkState_TwistLinear();
- tl->set_twistlinearx(msg.twist.at(link_number).linear.x);
- tl->set_twistlineary(msg.twist.at(link_number).linear.y);
- tl->set_twistlinearz(msg.twist.at(link_number).linear.z);
- pls_msg.set_allocated_tl(tl);
- }
-
- if (export_twist_angular) {
- panda::PandaLinkState_TwistAngular *ta = new panda::PandaLinkState_TwistAngular();
- ta->set_twistangularx(msg.twist.at(link_number).angular.x);
- ta->set_twistangulary(msg.twist.at(link_number).angular.y);
- ta->set_twistangularz(msg.twist.at(link_number).angular.z);
- pls_msg.set_allocated_ta(ta);
- }
-
- return pls_msg;
-}
-
-void setupMqtt() {
-
- if (!mqttSetup) {
- std::cout << "MQTT: Initializing." << std::endl;
-
- mqtt::connect_options connOpts;
- connOpts.set_keep_alive_interval(20);
- connOpts.set_clean_session(true);
- std::cout << "MQTT: Initialized." << std::endl;
-
- std::cout << "MQTT: Connecting." << std::endl;
- client.connect(connOpts);
- std::cout << "MQTT: Connected." << std::endl;
- mqttSetup = true;
- }
+ bool export_position = false;
+ bool export_orientation = false;
+ bool export_twist_linear = false;
+ bool export_twist_angular = false;
+
+ ros::NodeHandle n;
+
+ n.getParam("data_enable_position", export_position);
+ n.getParam("data_enable_orientation", export_orientation);
+ n.getParam("data_enable_twist_linear", export_twist_linear);
+ n.getParam("data_enable_twist_angular", export_twist_angular);
+
+ // std::cout << "Building LinkStateMessage with pos: " << export_position << " ori: " << export_orientation << "
+ // twistlin: "
+ // << export_twist_linear << " twistang: " << export_twist_angular << std::endl;
+
+ panda::PandaLinkState pls_msg;
+
+ pls_msg.set_name(msg.name.at(link_number));
+
+ if (export_position) {
+ panda::PandaLinkState_Position *pos = new panda::PandaLinkState_Position();
+ pos->set_positionx(msg.pose.at(link_number).position.x);
+ pos->set_positiony(msg.pose.at(link_number).position.y);
+ pos->set_positionz(msg.pose.at(link_number).position.z);
+ pls_msg.set_allocated_pos(pos);
+ }
+
+ if (export_orientation) {
+ panda::PandaLinkState_Orientation *orient = new panda::PandaLinkState_Orientation();
+ orient->set_orientationw(msg.pose.at(link_number).orientation.w);
+ orient->set_orientationx(msg.pose.at(link_number).orientation.x);
+ orient->set_orientationy(msg.pose.at(link_number).orientation.y);
+ orient->set_orientationz(msg.pose.at(link_number).orientation.z);
+ pls_msg.set_allocated_orient(orient);
+ }
+
+ if (export_twist_linear) {
+ panda::PandaLinkState_TwistLinear *tl = new panda::PandaLinkState_TwistLinear();
+ tl->set_twistlinearx(msg.twist.at(link_number).linear.x);
+ tl->set_twistlineary(msg.twist.at(link_number).linear.y);
+ tl->set_twistlinearz(msg.twist.at(link_number).linear.z);
+ pls_msg.set_allocated_tl(tl);
+ }
+
+ if (export_twist_angular) {
+ panda::PandaLinkState_TwistAngular *ta = new panda::PandaLinkState_TwistAngular();
+ ta->set_twistangularx(msg.twist.at(link_number).angular.x);
+ ta->set_twistangulary(msg.twist.at(link_number).angular.y);
+ ta->set_twistangularz(msg.twist.at(link_number).angular.z);
+ pls_msg.set_allocated_ta(ta);
+ }
+
+ return pls_msg;
}
-
void sendMqttMessages(const gazebo_msgs::LinkStates &msg) {
- setupMqtt();
-
+ if (mqttUtil.ensureConnection()) {
try {
- for (int i = 0; i < msg.name.size(); i++) {
- panda::PandaLinkState pls_msg = buildLinkStateMessage(msg, i);
- std::string mqtt_msg;
- pls_msg.SerializeToString(&mqtt_msg);
-
- //std::cout << "SENDING MESSAGE TO " << topics[i] << std::endl;
- auto pubmsg = mqtt::make_message(topics[i], mqtt_msg);
- pubmsg->set_qos(QOS);
- client.publish(pubmsg);
- }
+ for (int i = 0; i < msg.name.size(); i++) {
+ panda::PandaLinkState pls_msg = buildLinkStateMessage(msg, i);
+ std::string mqtt_msg;
+ pls_msg.SerializeToString(&mqtt_msg);
+
+ // std::cout << "SENDING MESSAGE TO " << topics[i] << std::endl;
+ auto pubmsg = mqtt::make_message(topics[i], mqtt_msg);
+ pubmsg->set_qos(QOS);
+ mqttUtil.getClient().publish(pubmsg);
+ }
}
// happens at lib paho sometimes when multiple instances of an mqtt-client are publishing at the same time
catch (const mqtt::exception &exc) {
- ROS_ERROR_STREAM("RSP: " << exc.what());
- // causes automatic reconnect
- mqttSetup = false;
- return;
+ ROS_ERROR_STREAM("RSP: " << exc.what());
+ return;
}
+ } else {
+ ROS_ERROR_STREAM_NAMED("MqttToRosNode", "Not connected! Unable to listen to messages.");
+ }
}
void providerCallback(const gazebo_msgs::LinkStates &msg) {
- ros::NodeHandle n;
- int message_redirect_rate = 200;
- n.getParam("data_publish_rate", message_redirect_rate);
-
- if (current_redirect != 0 && current_redirect != message_redirect_rate) {
- // std::cout << "Ignoring message (redirect: " << current_redirect << " )" << std::endl;
- current_redirect++;
- return;
- }
-
- if (current_redirect == 0 || current_redirect == message_redirect_rate) {
- //std::cout << "Sending LinkStates with rate: " << message_redirect_rate << std::endl;
+ // TODO the rate should not be depending on the link state rate, but rather be a time or frequency!!!!
- if (export_to_log) {
- exportMessageToLog(msg);
- }
+ ros::NodeHandle n;
+ int message_redirect_rate;
+ n.getParam("data_publish_rate", message_redirect_rate);
- sendMqttMessages(msg);
-
- if (current_redirect == message_redirect_rate) {
- current_redirect = 1;
- } else {
- current_redirect++;
- }
+ if (current_redirect == 0) {
+ if (export_to_log) {
+ exportMessageToLog(msg);
}
+ sendMqttMessages(msg);
+ }
+ current_redirect = ++current_redirect % message_redirect_rate;
}
int main(int argc, char **argv) {
- ros::init(argc, argv, "robot_state_provider");
- ros::NodeHandle n;
- n.getParam("ready", isInitialized);
+ ros::init(argc, argv, "robot_state_provider");
+ ros::NodeHandle n;
+ n.getParam("ready", isInitialized);
- while (ros::ok()) {
+ mqttUtil.connect();
- // make sure the robot is initialized
- while (!isInitialized) {
- //std::cout << "Waiting" << std::endl;
- n.getParam("tud_planner_ready", isInitialized);
- }
+ while (ros::ok()) {
- ros::Subscriber sub = n.subscribe("/gazebo/link_states", 10000, providerCallback);
- ros::spin();
+ // make sure the robot is initialized
+ while (!isInitialized) {
+ // std::cout << "Waiting" << std::endl;
+ n.getParam("tud_planner_ready", isInitialized);
}
- // disconnect from mqtt
- client.disconnect();
- return 0;
+ ros::Subscriber sub = n.subscribe("/gazebo/link_states", 10000, providerCallback);
+ ros::spin();
+ }
+
+ return 0;
}
\ No newline at end of file
diff --git a/src/SafetyAwarePlanner.cpp b/src/SafetyAwarePlanner.cpp
deleted file mode 100644
index afe69ecc2e5f16c385af1bcaf82bee58570e8253..0000000000000000000000000000000000000000
--- a/src/SafetyAwarePlanner.cpp
+++ /dev/null
@@ -1,104 +0,0 @@
-#include <moveit/move_group_interface/move_group_interface.h>
-#include <moveit/planning_scene_interface/planning_scene_interface.h>
-#include <moveit_msgs/CollisionObject.h>
-#include <moveit_visual_tools/moveit_visual_tools.h>
-
-#include "SafetyEnvironmentCreator.cpp"
-
-int main(int argc, char** argv)
-{
- ros::init(argc, argv, "Safety-aware PLANNER");
- ros::NodeHandle node_handle;
- ros::AsyncSpinner spinner(1);
- spinner.start();
-
- static const std::string PLANNING_GROUP = "panda_arm";
- moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
- moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
- const robot_state::JointModelGroup* joint_model_group =
- move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
-
- // Visualization Setup
- namespace rvt = rviz_visual_tools;
- moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
- //visual_tools.deleteAllMarkers();
- visual_tools.loadRemoteControl();
- visual_tools.setAlpha(0.5);
-
- Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
- text_pose.translation().z() = 1.75;
- visual_tools.publishText(text_pose, "Safety-aware planner node", rvt::WHITE, rvt::XLARGE);
-
- // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
- visual_tools.trigger();
-
- // Getting Basic Information
- ROS_INFO_NAMED("safety_aware_planner", "Planning frame: %s", move_group.getPlanningFrame().c_str());
- ROS_INFO_NAMED("safety_aware_planner", "End effector link: %s", move_group.getEndEffectorLink().c_str());
- ROS_INFO_NAMED("safety_aware_planner", "Available Planning Groups:");
- std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
- std::ostream_iterator<std::string>(std::cout, ", "));
-
- shape_msgs::SolidPrimitive safety_box;
- geometry_msgs::Pose safety_box_pose;
-
- safety_box_pose.orientation.w = 1.0;
- safety_box_pose.orientation.x = 0.0;
- safety_box_pose.orientation.y = 0.0;
- safety_box_pose.orientation.z = 0.0;
- safety_box_pose.position.x = 0.0;
- safety_box_pose.position.y = 0.0;
- safety_box_pose.position.z = 0.0;
-
- safety_box.type = safety_box.BOX;
- safety_box.dimensions.resize(3);
-
- safety_box.dimensions[0] = 2.0;
- safety_box.dimensions[1] = 2.0;
- safety_box.dimensions[2] = 2.0;
-
- // Safety Box
- SafetyEnvironmentCreator sec(safety_box, safety_box_pose);
- std::vector<moveit_msgs::CollisionObject> collision_objects = sec.createCentralizedSafetyEnvironment(move_group);
-
- //visual_tools.processCollisionObjectMsg(collision_objects[2], rviz_visual_tools::RED);
- // visual_tools.setAlpha(0.0);
- // Now, let's add the collision object into the world
- ROS_INFO_NAMED("safety_aware_planner", "Add an object into the world");
-
- planning_scene_interface.addCollisionObjects(collision_objects);
-
- // Show text in RViz of status
- visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);
- visual_tools.trigger();
- visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");
-
- // Now when we plan a trajectory it will avoid the obstacle
- move_group.setStartState(*move_group.getCurrentState());
- geometry_msgs::Pose another_pose;
- another_pose.orientation.w = 1.0;
- another_pose.position.x = 0.4;
- another_pose.position.y = -0.4;
- another_pose.position.z = 0.9;
- move_group.setPoseTarget(another_pose);
-
- //moveit_msgs::MotionPlanRequest mr;
- //mr.max_velocity_scaling_factor = 0.5;
- //move_group.constructMotionPlanRequest(mr);
-
- //move_group.setMaxVelocityScalingFactor(0.1);
- //move_group.setWorkspace(-0.5, -0.5, -0.5, 0, 0, 0);
-
- moveit::planning_interface::MoveGroupInterface::Plan my_plan;
- bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
- ROS_INFO_NAMED("safety_aware_planner", "Visualizing constraint plan %s", success ? "" : "FAILED");
-
- visual_tools.prompt("Press 'next' to move the simulated robot.");
- visual_tools.trigger();
-
- // Move the simulated robot in gazebo
- move_group.move();
-
- ros::shutdown();
- return 0;
-}
\ No newline at end of file
diff --git a/src/SafetyEnvironmentCreator.cpp b/src/SafetyEnvironmentCreator.cpp
deleted file mode 100644
index f6ccedb8084b4e384add979d5a65d05fd07e0b7d..0000000000000000000000000000000000000000
--- a/src/SafetyEnvironmentCreator.cpp
+++ /dev/null
@@ -1,270 +0,0 @@
-//
-// Created by Sebastian Ebert
-//
-
-#include <moveit/move_group_interface/move_group_interface.h>
-#include <moveit_msgs/CollisionObject.h>
-#include <moveit_visual_tools/moveit_visual_tools.h>
-
-class SafetyEnvironmentCreator {
-
- private:
- double safety_zone_width;
- double safety_zone_height;
- double safety_zone_depth;
- double zone_orientation_w;
- double zone_orientation_x;
- double zone_orientation_y;
- double zone_orientation_z;
- double zone_position_x;
- double zone_position_y;
- double zone_position_z;
-
- public:
- /**
- * ctor
- * @param shape describes the inner dimensions of the safety zone
- * @param pose describes position and orientation of the safety zone
- * note: current zone-threshold is 0.1
- */
- SafetyEnvironmentCreator(shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose) {
- if(shape.type != shape.BOX){
- ROS_ERROR("Safety-zone could not be created due to wrong shape-type");
- safety_zone_depth = 0.0;
- safety_zone_width = 0.0;
- safety_zone_height = 0.0;
- zone_orientation_w = 1.0;
- zone_orientation_x = 1.0;
- zone_orientation_y = 1.0;
- zone_orientation_z = 1.0;
- }else {
- safety_zone_depth = shape.dimensions[0];
- safety_zone_width = shape.dimensions[1];
- safety_zone_height = shape.dimensions[2];
- zone_orientation_w = pose.orientation.w;
- zone_orientation_x = pose.orientation.x;
- zone_orientation_y = pose.orientation.y;
- zone_orientation_z = pose.orientation.z;
- zone_position_x = pose.position.x;
- zone_position_y = pose.position.y;
- zone_position_z = pose.position.z;
- }
- }
-
- double getZoneWidth() const {
- return safety_zone_width;
- }
-
- double getZoneHeight() const {
- return safety_zone_height;
- }
-
- double getZoneDepth() const {
- return safety_zone_depth;
- }
-
- double getZoneOrientationW() const {
- return zone_orientation_w;
- }
-
- double getZoneOrientationX() const {
- return zone_orientation_x;
- }
-
- double getZoneOrientationY() const {
- return zone_orientation_y;
- }
-
- double getZoneOrientationZ() const {
- return zone_orientation_z;
- }
-
- std::vector<moveit_msgs::CollisionObject> createCentralizedSafetyEnvironment(moveit::planning_interface::MoveGroupInterface &move_group) {
-
- std::vector<moveit_msgs::CollisionObject> collision_objects;
-
- // OBJECT 1 ( left box / -y )
- // ^^^^^^^^
- // Define a collision object ROS message.
- moveit_msgs::CollisionObject collision_object;
- collision_object.header.frame_id = move_group.getPlanningFrame();
-
- // The id of the object is used to identify it.
- collision_object.id = "box1";
-
- // Define a box to add to the world.
- shape_msgs::SolidPrimitive primitive;
- primitive.type = primitive.BOX;
- primitive.dimensions.resize(3);
-
- primitive.dimensions[0] = getZoneDepth() + 0.2; // x
- primitive.dimensions[1] = 0.1; // y
- primitive.dimensions[2] = getZoneHeight(); // z
-
- // Define a pose for the box (specified relative to frame_id)
- geometry_msgs::Pose box_pose;
- box_pose.orientation.w = 1.0;
- box_pose.position.x = zone_position_x;
- box_pose.position.y = zone_position_y + (-1 * ((getZoneWidth() / 2) + 0.05));
- box_pose.position.z = zone_position_z + (primitive.dimensions[2] / 2);
-
- collision_object.primitives.push_back(primitive);
- collision_object.primitive_poses.push_back(box_pose);
- collision_object.operation = collision_object.ADD;
-
- collision_objects.push_back(collision_object);
-
- // OBJECT 2 right box / +y )
- // ^^^^^^^^
- moveit_msgs::CollisionObject collision_object_2;
- collision_object_2.header.frame_id = move_group.getPlanningFrame();
-
- // The id of the object is used to identify it.
- collision_object_2.id = "box2";
-
- // Define a box to add to the world.
- shape_msgs::SolidPrimitive primitive_2;
- primitive_2.type = primitive.BOX;
- primitive_2.dimensions.resize(3);
- primitive_2.dimensions[0] = getZoneDepth() + 0.2;
- primitive_2.dimensions[1] = 0.1;
- primitive_2.dimensions[2] = getZoneHeight();
-
- // Define a pose for the box (specified relative to frame_id)
- geometry_msgs::Pose box_pose_2;
- box_pose_2.orientation.w = 1.0;
- box_pose_2.position.x = zone_position_x;
- box_pose_2.position.y = zone_position_y + ((getZoneWidth() / 2) + 0.05);
- box_pose_2.position.z = zone_position_z + (primitive_2.dimensions[2] / 2);
-
- collision_object_2.primitives.push_back(primitive_2);
- collision_object_2.primitive_poses.push_back(box_pose_2);
- collision_object_2.operation = collision_object_2.ADD;
-
- collision_objects.push_back(collision_object_2);
-
- // OBJECT 3 (front box / +x)
- // ^^^^^^^^
- moveit_msgs::CollisionObject collision_object_3;
- collision_object_3.header.frame_id = move_group.getPlanningFrame();
-
- // The id of the object is used to identify it.
- collision_object_3.id = "box3";
-
- // Define a box to add to the world.
- shape_msgs::SolidPrimitive primitive_3;
- primitive_3.type = primitive.BOX;
- primitive_3.dimensions.resize(3);
- primitive_3.dimensions[0] = 0.1;
- primitive_3.dimensions[1] = getZoneWidth() + 0.2;
- primitive_3.dimensions[2] = getZoneHeight();
-
- // Define a pose for the box (specified relative to frame_id)
- geometry_msgs::Pose box_pose_3;
- box_pose_3.orientation.w = 1.0;
- box_pose_3.position.x = zone_position_x + ((getZoneDepth() / 2) + 0.05);
- box_pose_3.position.y = zone_position_y;
- box_pose_3.position.z = zone_position_z + (primitive_3.dimensions[2] / 2);
-
- collision_object_3.primitives.push_back(primitive_3);
- collision_object_3.primitive_poses.push_back(box_pose_3);
- collision_object_3.operation = collision_object_3.ADD;
-
- collision_objects.push_back(collision_object_3);
-
- // OBJECT 4 (back box / -x)
- // ^^^^^^^^
- moveit_msgs::CollisionObject collision_object_4;
- collision_object_4.header.frame_id = move_group.getPlanningFrame();
-
- // The id of the object is used to identify it.
- collision_object_4.id = "box4";
-
- // Define a box to add to the world.
- shape_msgs::SolidPrimitive primitive_4;
- primitive_4.type = primitive.BOX;
- primitive_4.dimensions.resize(3);
- primitive_4.dimensions[0] = 0.1;
- primitive_4.dimensions[1] = getZoneWidth() + 0.2;
- primitive_4.dimensions[2] = getZoneHeight();
-
- // Define a pose for the box (specified relative to frame_id)
- geometry_msgs::Pose box_pose_4;
- box_pose_4.orientation.w = 1.0;
- box_pose_4.position.x = zone_position_x + (-1 * ((getZoneDepth() / 2) + 0.05));
- box_pose_4.position.y = zone_position_y;
- box_pose_4.position.z = zone_position_z + (primitive_4.dimensions[2] / 2);
-
- collision_object_4.primitives.push_back(primitive_4);
- collision_object_4.primitive_poses.push_back(box_pose_4);
- collision_object_4.operation = collision_object_4.ADD;
-
- collision_objects.push_back(collision_object_4);
-
- // OBJECT 5 (floor / -z)
- // ^^^^^^^^
- moveit_msgs::CollisionObject collision_object_5;
- collision_object_5.header.frame_id = move_group.getPlanningFrame();
-
- // The id of the object is used to identify it.
- collision_object_5.id = "box5";
-
- // Define a box to add to the world.
- shape_msgs::SolidPrimitive primitive_5;
- primitive_5.type = primitive.BOX;
- primitive_5.dimensions.resize(3);
- primitive_5.dimensions[0] = getZoneDepth() + 0.2;
- primitive_5.dimensions[1] = getZoneWidth() + 0.2;
- primitive_5.dimensions[2] = 0.1;
-
- // Define a pose for the box (specified relative to frame_id)
- geometry_msgs::Pose box_pose_5;
- box_pose_5.orientation.w = 1.0;
- box_pose_5.position.x = zone_position_x;
- box_pose_5.position.y = zone_position_y;
- box_pose_5.position.z = zone_position_z - 0.05;
-
- collision_object_5.primitives.push_back(primitive_5);
- collision_object_5.primitive_poses.push_back(box_pose_5);
- collision_object_5.operation = collision_object_5.ADD;
-
- collision_objects.push_back(collision_object_5);
-
- // OBJECT 6 (top / +z)
- // ^^^^^^^^
- moveit_msgs::CollisionObject collision_object_6;
- collision_object_6.header.frame_id = move_group.getPlanningFrame();
-
- // The id of the object is used to identify it.
- collision_object_6.id = "box6";
-
- // Define a box to add to the world.
- shape_msgs::SolidPrimitive primitive_6;
- primitive_6.type = primitive.BOX;
- primitive_6.dimensions.resize(3);
- primitive_6.dimensions[0] = getZoneDepth() + 0.2;
- primitive_6.dimensions[1] = getZoneWidth() + 0.2;
- primitive_6.dimensions[2] = 0.1;
-
- // Define a pose for the box (specified relative to frame_id)
- geometry_msgs::Pose box_pose_6;
- box_pose_6.orientation.w = 1.0;
- box_pose_6.position.x = zone_position_x;
- box_pose_6.position.y = zone_position_y;
- box_pose_6.position.z = zone_position_z + getZoneHeight() + 0.05;
-
- collision_object_6.primitives.push_back(primitive_6);
- collision_object_6.primitive_poses.push_back(box_pose_6);
- collision_object_6.operation = collision_object_6.ADD;
- collision_objects.push_back(collision_object_6);
-
- /*for(std::size_t i=0; i<collision_objects.size(); ++i) {
- collision_objects[i].primitive_poses[0].orientation.w = getZoneOrientationW();
- collision_objects[i].primitive_poses[0].orientation.x = getZoneOrientationX();
- collision_objects[i].primitive_poses[0].orientation.y = getZoneOrientationY();
- collision_objects[i].primitive_poses[0].orientation.z = getZoneOrientationZ();
- }*/
-
- return collision_objects;
- }
-};
\ No newline at end of file
diff --git a/src/TimedPlanner.cpp b/src/TimedPlanner.cpp
index 97ead42011a09bb2f103d0d09048932383771e98..5d3005c0fc59b52098ab67076be66e6835355abc 100644
--- a/src/TimedPlanner.cpp
+++ b/src/TimedPlanner.cpp
@@ -158,17 +158,6 @@ int main(int argc, char **argv) {
ROS_INFO(">>>>>>>>>>>>>>>>> START UP FINISHED <<<<<<<<<<<<<<<< ");
node_handle.setParam("tud_planner_ready", true);
- // Visualization Setup.
- namespace rvt = rviz_visual_tools;
- moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
- visual_tools.deleteAllMarkers();
- visual_tools.loadRemoteControl();
-
- Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
- text_pose.translation().z() = 1.75;
- visual_tools.publishText(text_pose, "cartesian planner node", rvt::WHITE, rvt::XLARGE);
- visual_tools.trigger();
-
// Initialize start state of robot and target trajectory.
moveit::planning_interface::MoveGroupInterface group("panda_arm");
moveRobotToInitialState(node_handle);
diff --git a/src/mem_persistence.cpp b/src/mem_persistence.cpp
deleted file mode 100644
index b6da0550a59c174145d2b831b4bcc978782b63fa..0000000000000000000000000000000000000000
--- a/src/mem_persistence.cpp
+++ /dev/null
@@ -1,71 +0,0 @@
-#include <string>
-#include <map>
-#include <vector>
-#include "mqtt/client.h"
-
-// Connection persistence class.
-class mem_persistence : virtual public mqtt::iclient_persistence {
- // Whether the store is open
- bool open_;
-
- // Use an STL map to store shared persistence pointers
- // against string keys.
- std::map<std::string, std::string> store_;
-
-public:
- mem_persistence() : open_(false) {}
-
- // "Open" the store
- void open(const std::string &clientId, const std::string &serverURI) override {
- open_ = true;
- }
-
- // Close the persistent store that was previously opened.
- void close() override {
- open_ = false;
- }
-
- // Clears persistence, so that it no longer contains any persisted data.
- void clear() override {
- store_.clear();
- }
-
- // Returns whether or not data is persisted using the specified key.
- bool contains_key(const std::string &key) override {
- return store_.find(key) != store_.end();
- }
-
- // Returns the keys in this persistent data store.
- const mqtt::string_collection &keys() const override {
- static mqtt::string_collection ks;
- ks.clear();
- for (const auto &k : store_)
- ks.push_back(k.first);
- return ks;
- }
-
- // Puts the specified data into the persistent store.
- void put(const std::string &key, const std::vector<mqtt::string_view> &bufs) override {
- std::string str;
- for (const auto &b : bufs)
- str += b.str();
- store_[key] = std::move(str);
- }
-
- // Gets the specified data out of the persistent store.
- mqtt::string_view get(const std::string &key) const override {
- auto p = store_.find(key);
- if (p == store_.end())
- throw mqtt::persistence_exception();
-
- return mqtt::string_view(p->second);
- }
-
- // Remove the data for the specified key.
- void remove(const std::string &key) override {
- auto p = store_.find(key);
- if (p == store_.end())
- throw mqtt::persistence_exception();
- store_.erase(p);;
- }
-};
\ No newline at end of file
diff --git a/src/robot_control_node.cpp b/src/robot_control_node.cpp
deleted file mode 100644
index beb175b32de85e11c68c7227be65ff7dcdcb068c..0000000000000000000000000000000000000000
--- a/src/robot_control_node.cpp
+++ /dev/null
@@ -1,140 +0,0 @@
-#include <jsoncpp/json/json.h>
-#include <moveit/move_group_interface/move_group_interface.h>
-#include <moveit/planning_scene/planning_scene.h>
-#include <moveit/planning_scene_interface/planning_scene_interface.h>
-#include <moveit_visual_tools/moveit_visual_tools.h>
-#include <ros/ros.h>
-#include <boost/filesystem.hpp>
-
-static const std::string PLANNING_GROUP_ARM = "panda_arm";
-static const std::string APP_DIRECTORY_NAME = ".panda_simulation";
-
-moveit_msgs::CollisionObject extractObstacleFromJson(Json::Value &root, std::string name)
-{
- moveit_msgs::CollisionObject collision_object;
- collision_object.header.frame_id = "world";
- collision_object.id = name;
-
- const Json::Value dimensions = root["dimensions"];
- ROS_INFO_STREAM("Extracted dimensions: " << dimensions);
- // Define a box to add to the world.
- shape_msgs::SolidPrimitive primitive;
- primitive.type = primitive.BOX;
- primitive.dimensions.resize(3);
- primitive.dimensions[0] = dimensions["x"].asDouble();
- primitive.dimensions[1] = dimensions["y"].asDouble();
- primitive.dimensions[2] = dimensions["z"].asDouble();
-
- const Json::Value position = root["position"];
- ROS_INFO_STREAM("Extracted position: " << position);
-
- const Json::Value orientation = root["orientation"];
- ROS_INFO_STREAM("Extracted orientation: " << orientation);
- // Define a pose for the box (specified relative to frame_id)
- geometry_msgs::Pose box_pose;
- box_pose.orientation.w = orientation["w"].asDouble();
- box_pose.orientation.x = orientation["x"].asDouble();
- box_pose.orientation.y = orientation["y"].asDouble();
- box_pose.orientation.z = orientation["z"].asDouble();
-
- // MoveIt! planning scene expects the center of the object as position.
- // We add half of its dimension to its position
- box_pose.position.x = position["x"].asDouble() + primitive.dimensions[0] / 2.0;
- box_pose.position.y = position["y"].asDouble() + primitive.dimensions[1] / 2.0;
- box_pose.position.z = position["z"].asDouble() + primitive.dimensions[2] / 2.0;
-
- collision_object.primitives.push_back(primitive);
- collision_object.primitive_poses.push_back(box_pose);
- collision_object.operation = collision_object.ADD;
-
- return std::move(collision_object);
-}
-
-int main(int argc, char **argv)
-{
- namespace fs = boost::filesystem;
- ROS_INFO("RUNNING robot_control_node");
-
- ros::init(argc, argv, "robot_control_node");
-
- ros::NodeHandle node_handle;
- ros::AsyncSpinner spinner(1);
- spinner.start();
-
- moveit::planning_interface::MoveGroupInterface move_group_arm(PLANNING_GROUP_ARM);
-
- ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
- ros::WallDuration sleep_t(0.5);
- while (planning_scene_diff_publisher.getNumSubscribers() < 1)
- {
- sleep_t.sleep();
- }
- moveit_msgs::PlanningScene planning_scene;
-
- // read JSON files from ~/.panda_simulation
- fs::path home(getenv("HOME"));
- if (fs::is_directory(home))
- {
- fs::path app_directory(home);
- app_directory /= APP_DIRECTORY_NAME;
-
- if (!fs::exists(app_directory) && !fs::is_directory(app_directory))
- {
- ROS_WARN_STREAM(app_directory << " does not exist");
-
- // Create .panda_simulation directory
- std::string path(getenv("HOME"));
- path += "/.panda_simulation";
- ROS_INFO("Creating %s collision objects directory.", path);
- try
- {
- boost::filesystem::create_directory(path);
- }
- catch (const std::exception&)
- {
- ROS_ERROR(
- "%s directory could not be created."
- "Please create this directory yourself "
- "if you want to specify collision objects.", path.c_str());
- return -1;
- }
- }
-
- std::vector<moveit_msgs::CollisionObject> collision_objects;
- ROS_INFO_STREAM(app_directory << " is a directory containing:");
- for (auto &entry : boost::make_iterator_range(fs::directory_iterator(app_directory), {}))
- {
- ROS_INFO_STREAM(entry);
-
- std::ifstream file_stream(entry.path().string(), std::ifstream::binary);
- if (file_stream)
- {
- Json::Value root;
- file_stream >> root;
-
- moveit_msgs::CollisionObject collision_object = extractObstacleFromJson(root, entry.path().stem().string());
- collision_objects.push_back(collision_object);
- }
- else
- {
- ROS_WARN_STREAM("could not open file " << entry.path());
- }
- }
-
- // Publish the collision objects to the scene
- for (const auto &collision_object : collision_objects)
- {
- collision_object.header.frame_id = move_group_arm.getPlanningFrame();
- planning_scene.world.collision_objects.push_back(collision_object);
- }
-
- ROS_INFO_STREAM("# collision objects " << planning_scene.world.collision_objects.size());
- planning_scene.is_diff = true;
- planning_scene_diff_publisher.publish(planning_scene);
-
- ROS_INFO("robot_control_node is ready");
- ros::waitForShutdown();
-
- return 0;
- }
-}
\ No newline at end of file
diff --git a/src/samples/SampleConstraintPlanner.cpp b/src/samples/SampleConstraintPlanner.cpp
deleted file mode 100644
index 4f442184ef3d8d1a11189f1bce2cbff7f8ed8847..0000000000000000000000000000000000000000
--- a/src/samples/SampleConstraintPlanner.cpp
+++ /dev/null
@@ -1,109 +0,0 @@
-//
-// Created by sebastian on 27.03.20.
-//
-#include <moveit/move_group_interface/move_group_interface.h>
-#include <moveit/planning_scene_interface/planning_scene_interface.h>
-
-#include <moveit_msgs/DisplayRobotState.h>
-#include <moveit_msgs/DisplayTrajectory.h>
-
-#include <moveit_msgs/AttachedCollisionObject.h>
-#include <moveit_msgs/CollisionObject.h>
-
-#include <moveit_visual_tools/moveit_visual_tools.h>
-#include <moveit/trajectory_processing/iterative_time_parameterization.h>
-#include <trajectory_msgs/JointTrajectoryPoint.h>
-
-int main(int argc, char** argv)
-{
- ros::init(argc, argv, "CONSTRAINT PLANNER");
- ros::NodeHandle node_handle;
- ros::AsyncSpinner spinner(1);
- spinner.start();
-
- static const std::string PLANNING_GROUP = "panda_arm";
- moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
- moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
- const robot_state::JointModelGroup* joint_model_group =
- move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
-
- // Visualization Setup
- namespace rvt = rviz_visual_tools;
- moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
- visual_tools.deleteAllMarkers();
- visual_tools.loadRemoteControl();
-
- Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
- text_pose.translation().z() = 1.75;
- visual_tools.publishText(text_pose, "Sample constraint planner node", rvt::WHITE, rvt::XLARGE);
-
- // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
- visual_tools.trigger();
-
- // Getting Basic Information
- ROS_INFO_NAMED("constraint_planner", "Planning frame: %s", move_group.getPlanningFrame().c_str());
- ROS_INFO_NAMED("constraint_planner", "End effector link: %s", move_group.getEndEffectorLink().c_str());
- ROS_INFO_NAMED("constraint_planner", "Available Planning Groups:");
- std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
- std::ostream_iterator<std::string>(std::cout, ", "));
-
- // Define a collision object ROS message.
- moveit_msgs::CollisionObject collision_object;
- collision_object.header.frame_id = move_group.getPlanningFrame();
-
- // The id of the object is used to identify it.
- collision_object.id = "box1";
-
- // Define a box to add to the world.
- shape_msgs::SolidPrimitive primitive;
- primitive.type = primitive.BOX;
- primitive.dimensions.resize(3);
- primitive.dimensions[0] = 0.4;
- primitive.dimensions[1] = 0.1;
- primitive.dimensions[2] = 0.4;
-
- // Define a pose for the box (specified relative to frame_id)
- geometry_msgs::Pose box_pose;
- box_pose.orientation.w = 1.0;
- box_pose.position.x = 0.4;
- box_pose.position.y = -0.2;
- box_pose.position.z = 1.0;
-
- collision_object.primitives.push_back(primitive);
- collision_object.primitive_poses.push_back(box_pose);
- collision_object.operation = collision_object.ADD;
-
- std::vector<moveit_msgs::CollisionObject> collision_objects;
- collision_objects.push_back(collision_object);
-
- // Now, let's add the collision object into the world
- ROS_INFO_NAMED("constraint_planner", "Add an object into the world");
- planning_scene_interface.addCollisionObjects(collision_objects);
-
- // Show text in RViz of status
- visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);
- visual_tools.trigger();
- visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");
-
- // Now when we plan a trajectory it will avoid the obstacle
- move_group.setStartState(*move_group.getCurrentState());
- geometry_msgs::Pose another_pose;
- another_pose.orientation.w = 1.0;
- another_pose.position.x = 0.4;
- another_pose.position.y = -0.4;
- another_pose.position.z = 0.9;
- move_group.setPoseTarget(another_pose);
-
- moveit::planning_interface::MoveGroupInterface::Plan my_plan;
- bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
- ROS_INFO_NAMED("constraint_planner", "Visualizing constraint plan %s", success ? "" : "FAILED");
-
- visual_tools.prompt("Press 'next' to move the simulated robot.");
- visual_tools.trigger();
-
- // Move the simulated robot in gazebo
- move_group.move();
-
- ros::shutdown();
- return 0;
-}
\ No newline at end of file
diff --git a/src/samples/SampleTimedCartesianPlanner.cpp b/src/samples/SampleTimedCartesianPlanner.cpp
deleted file mode 100644
index cf8d1cff3e77c9798f01a7bb34704a077f584eb1..0000000000000000000000000000000000000000
--- a/src/samples/SampleTimedCartesianPlanner.cpp
+++ /dev/null
@@ -1,142 +0,0 @@
-//
-// Created by sebastian on 31.03.20.
-//
-#include <moveit/move_group_interface/move_group_interface.h>
-#include <moveit/planning_scene_interface/planning_scene_interface.h>
-
-#include <moveit_msgs/DisplayRobotState.h>
-#include <moveit_msgs/DisplayTrajectory.h>
-
-#include <moveit_msgs/AttachedCollisionObject.h>
-#include <moveit_msgs/CollisionObject.h>
-
-#include <moveit_visual_tools/moveit_visual_tools.h>
-#include <moveit/trajectory_processing/iterative_time_parameterization.h>
-#include <trajectory_msgs/JointTrajectoryPoint.h>
-
-/**
- * allows time/velocity-constraint planning for cartesian paths
- */
-int main(int argc, char **argv)
-{
- ros::init(argc, argv, "move_group_interface_tutorial");
- ros::NodeHandle node_handle;
- ros::AsyncSpinner spinner(1);
- spinner.start();
-
- // wait for robot init of robot_state_initializer
- std::cout << ">>>>>>>>>>>>>>>>> SLEEPING <<<<<<<<<<<<<<<< " << std::endl;
- ros::Duration(5.0).sleep();
- std::cout << ">>>>>>>>>>>>>>>>> WAKING UP <<<<<<<<<<<<<<<< " << std::endl;
- node_handle.setParam("tud_planner_ready", true);
-
- // Visualization Setup
- namespace rvt = rviz_visual_tools;
- moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
- visual_tools.deleteAllMarkers();
- visual_tools.loadRemoteControl();
-
- Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
- text_pose.translation().z() = 1.75;
- visual_tools.publishText(text_pose, "Sample constraint planner node", rvt::WHITE, rvt::XLARGE);
-
- // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
- visual_tools.trigger();
-
- moveit::planning_interface::MoveGroupInterface group("panda_arm");
-
- moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
-
- ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
- moveit_msgs::DisplayTrajectory display_trajectory;
-
- moveit::planning_interface::MoveGroupInterface::Plan plan;
- group.setStartStateToCurrentState();
-
- std::vector<geometry_msgs::Pose> waypoints;
-
- // initial pose of the robot
- geometry_msgs::Pose start_pose = group.getCurrentPose().pose;
-
- // waypoints for a circle
- geometry_msgs::Pose target_pose_1 = group.getCurrentPose().pose;
- geometry_msgs::Pose target_pose_2 = group.getCurrentPose().pose;
- geometry_msgs::Pose target_pose_3 = group.getCurrentPose().pose;
- geometry_msgs::Pose target_pose_4 = group.getCurrentPose().pose;
- geometry_msgs::Pose target_pose_5 = group.getCurrentPose().pose;
-
- // specification of the circles waypoints
- target_pose_1.position.z = start_pose.position.z - 0.4;
- target_pose_1.position.y = start_pose.position.y;
- target_pose_1.position.x = start_pose.position.x + 0.5;
- waypoints.push_back(target_pose_1);
-
- target_pose_2.position.z = 0.6;//0.583;
- target_pose_2.position.y = -0.6;//63;
- target_pose_2.position.x = 0;//-0.007;
- waypoints.push_back(target_pose_2);
-
- target_pose_3.position.z = 0.6;//0.691;
- target_pose_3.position.y = -0.032;
- target_pose_3.position.x = -0.607;
- waypoints.push_back(target_pose_3);
-
- target_pose_4.position.z = 0.6;
- target_pose_4.position.y = 0.6;//0.509;
- target_pose_4.position.x = 0;//0.039;
- waypoints.push_back(target_pose_4);
-
- target_pose_5.position.z = target_pose_1.position.z;
- target_pose_5.position.y = target_pose_1.position.y;
- target_pose_5.position.x = target_pose_1.position.x;
- waypoints.push_back(target_pose_5);
-
- moveit_msgs::RobotTrajectory trajectory_msg;
- group.setPlanningTime(10.0);
-
- double fraction = group.computeCartesianPath(waypoints,0.01,0.0,trajectory_msg, false);
-
- // The trajectory needs to be modified so it will include velocities as well.
- // First to create a RobotTrajectory object
- robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "panda_arm");
-
- // Second get a RobotTrajectory from trajectory
- rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg);
-
- // Third create a IterativeParabolicTimeParameterization object
- trajectory_processing::IterativeParabolicTimeParameterization iptp;
- // Fourth compute computeTimeStamps
- bool success = iptp.computeTimeStamps(rt);
- ROS_INFO("Computed time stamp %s",success?"SUCCEDED":"FAILED");
- rt.getRobotTrajectoryMsg(trajectory_msg);
-
- //std::cout << ">>>>>>>>>>>>>>>>> data before: " << trajectory_msg << std::endl;
-
- // adjust velocities, accelerations and time_from_start
- for(int i = 0; i < trajectory_msg.joint_trajectory.points.size(); i++)
- {
- trajectory_msg.joint_trajectory.points[i].time_from_start.operator*=(2);
- for(int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).velocities.size(); j++)
- {
- if(trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) != 0.0){
- trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) /= 2;
- //trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) = 0;
- }
- }
-
- for(int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).accelerations.size(); j++)
- {
- trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j) =
- sqrt(std::abs(trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j)));
- }
- }
-
- //std::cout << ">>>>>>>>>>>>>>>>> data after: " << trajectory_msg << std::endl;
- plan.trajectory_ = trajectory_msg;
- ROS_INFO("Visualizing plan 4 (cartesian path) (%.2f%% acheived)",fraction * 100.0);
-
- group.execute(plan);
-
- ros::shutdown();
- return 0;
-}
\ No newline at end of file
diff --git a/src/tutorial/motion_planning_api_tutorial.cpp b/src/tutorial/motion_planning_api_tutorial.cpp
deleted file mode 100644
index 9e95aadb8f7f14d75eeb88ef81d5faf1555f1c2a..0000000000000000000000000000000000000000
--- a/src/tutorial/motion_planning_api_tutorial.cpp
+++ /dev/null
@@ -1,352 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2012, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-/* Author: Sachin Chitta, Michael Lautman */
-
-#include <pluginlib/class_loader.h>
-#include <ros/ros.h>
-
-// MoveIt
-#include <moveit/robot_model_loader/robot_model_loader.h>
-#include <moveit/planning_interface/planning_interface.h>
-#include <moveit/planning_scene/planning_scene.h>
-#include <moveit/kinematic_constraints/utils.h>
-#include <moveit_msgs/DisplayTrajectory.h>
-#include <moveit_msgs/PlanningScene.h>
-#include <moveit_visual_tools/moveit_visual_tools.h>
-
-#include <boost/scoped_ptr.hpp>
-
-int main(int argc, char** argv)
-{
- const std::string node_name = "motion_planning_tutorial";
- ros::init(argc, argv, node_name);
- ros::AsyncSpinner spinner(1);
- spinner.start();
- ros::NodeHandle node_handle("~");
-
- // BEGIN_TUTORIAL
- // Start
- // ^^^^^
- // Setting up to start using a planner is pretty easy. Planners are
- // setup as plugins in MoveIt and you can use the ROS pluginlib
- // interface to load any planner that you want to use. Before we
- // can load the planner, we need two objects, a RobotModel and a
- // PlanningScene. We will start by instantiating a `RobotModelLoader`_
- // object, which will look up the robot description on the ROS
- // parameter server and construct a :moveit_core:`RobotModel` for us
- // to use.
- //
- // .. _RobotModelLoader:
- // http://docs.ros.org/indigo/api/moveit_ros_planning/html/classrobot__model__loader_1_1RobotModelLoader.html
- const std::string PLANNING_GROUP = "panda_arm";
- robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
- robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();
- /* Create a RobotState and JointModelGroup to keep track of the current robot pose and planning group*/
- robot_state::RobotStatePtr robot_state(new robot_state::RobotState(robot_model));
- const robot_state::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP);
-
- // Using the :moveit_core:`RobotModel`, we can construct a :planning_scene:`PlanningScene`
- // that maintains the state of the world (including the robot).
- planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));
-
- // Configure a valid robot state
- planning_scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "ready");
-
- // We will now construct a loader to load a planner, by name.
- // Note that we are using the ROS pluginlib library here.
- boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager>> planner_plugin_loader;
- planning_interface::PlannerManagerPtr planner_instance;
- std::string planner_plugin_name;
-
- // We will get the name of planning plugin we want to load
- // from the ROS parameter server, and then load the planner
- // making sure to catch all exceptions.
- if (!node_handle.getParam("planning_plugin", planner_plugin_name))
- ROS_FATAL_STREAM("Could not find planner plugin name");
- try
- {
- planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>(
- "moveit_core", "planning_interface::PlannerManager"));
- }
- catch (pluginlib::PluginlibException& ex)
- {
- ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what());
- }
- try
- {
- planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name));
- if (!planner_instance->initialize(robot_model, node_handle.getNamespace()))
- ROS_FATAL_STREAM("Could not initialize planner instance");
- ROS_INFO_STREAM("Using planning interface '" << planner_instance->getDescription() << "'");
- }
- catch (pluginlib::PluginlibException& ex)
- {
- const std::vector<std::string>& classes = planner_plugin_loader->getDeclaredClasses();
- std::stringstream ss;
- for (std::size_t i = 0; i < classes.size(); ++i)
- ss << classes[i] << " ";
- ROS_ERROR_STREAM("Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl
- << "Available plugins: " << ss.str());
- }
-
- // Visualization
- // ^^^^^^^^^^^^^
- // The package MoveItVisualTools provides many capabilities for visualizing objects, robots,
- // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script.
- namespace rvt = rviz_visual_tools;
- moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
- visual_tools.loadRobotStatePub("/display_robot_state");
- visual_tools.enableBatchPublishing();
- visual_tools.deleteAllMarkers(); // clear all old markers
- visual_tools.trigger();
-
- /* Remote control is an introspection tool that allows users to step through a high level script
- via buttons and keyboard shortcuts in RViz */
- visual_tools.loadRemoteControl();
-
- /* RViz provides many types of markers, in this demo we will use text, cylinders, and spheres*/
- Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
- text_pose.translation().z() = 1.75;
- visual_tools.publishText(text_pose, "Motion Planning API Demo", rvt::WHITE, rvt::XLARGE);
-
- /* Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations */
- visual_tools.trigger();
-
- /* We can also use visual_tools to wait for user input */
- visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");
-
- // Pose Goal
- // ^^^^^^^^^
- // We will now create a motion plan request for the arm of the Panda
- // specifying the desired pose of the end-effector as input.
- visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN);
- visual_tools.trigger();
- planning_interface::MotionPlanRequest req;
- planning_interface::MotionPlanResponse res;
- geometry_msgs::PoseStamped pose;
- pose.header.frame_id = "panda_link0";
- pose.pose.position.x = 0.3;
- pose.pose.position.y = 0.4;
- pose.pose.position.z = 0.75;
- pose.pose.orientation.w = 1.0;
-
- // A tolerance of 0.01 m is specified in position
- // and 0.01 radians in orientation
- std::vector<double> tolerance_pose(3, 0.01);
- std::vector<double> tolerance_angle(3, 0.01);
-
- // We will create the request as a constraint using a helper function available
- // from the
- // `kinematic_constraints`_
- // package.
- //
- // .. _kinematic_constraints:
- // http://docs.ros.org/indigo/api/moveit_core/html/namespacekinematic__constraints.html#a88becba14be9ced36fefc7980271e132
- moveit_msgs::Constraints pose_goal =
- kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle);
-
- req.group_name = PLANNING_GROUP;
- req.goal_constraints.push_back(pose_goal);
-
- // We now construct a planning context that encapsulate the scene,
- // the request and the response. We call the planner using this
- // planning context
- planning_interface::PlanningContextPtr context =
- planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
- context->solve(res);
- if (res.error_code_.val != res.error_code_.SUCCESS)
- {
- ROS_ERROR("Could not compute plan successfully");
- return 0;
- }
-
- // Visualize the result
- // ^^^^^^^^^^^^^^^^^^^^
- ros::Publisher display_publisher =
- node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
- moveit_msgs::DisplayTrajectory display_trajectory;
-
- /* Visualize the trajectory */
- moveit_msgs::MotionPlanResponse response;
- res.getMessage(response);
-
- display_trajectory.trajectory_start = response.trajectory_start;
- display_trajectory.trajectory.push_back(response.trajectory);
- visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
- visual_tools.trigger();
- display_publisher.publish(display_trajectory);
-
- /* Set the state in the planning scene to the final state of the last plan */
- robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
- planning_scene->setCurrentState(*robot_state.get());
-
- // Display the goal state
- visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN);
- visual_tools.publishAxisLabeled(pose.pose, "goal_1");
- visual_tools.publishText(text_pose, "Pose Goal (1)", rvt::WHITE, rvt::XLARGE);
- visual_tools.trigger();
-
- /* We can also use visual_tools to wait for user input */
- visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
-
- // Joint Space Goals
- // ^^^^^^^^^^^^^^^^^
- // Now, setup a joint space goal
- robot_state::RobotState goal_state(robot_model);
- std::vector<double> joint_values = { -1.0, 0.7, 0.7, -1.5, -0.7, 2.0, 0.0 };
- goal_state.setJointGroupPositions(joint_model_group, joint_values);
- moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group);
- req.goal_constraints.clear();
- req.goal_constraints.push_back(joint_goal);
-
- // Call the planner and visualize the trajectory
- /* Re-construct the planning context */
- context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
- /* Call the Planner */
- context->solve(res);
- /* Check that the planning was successful */
- if (res.error_code_.val != res.error_code_.SUCCESS)
- {
- ROS_ERROR("Could not compute plan successfully");
- return 0;
- }
- /* Visualize the trajectory */
- res.getMessage(response);
- display_trajectory.trajectory.push_back(response.trajectory);
-
- /* Now you should see two planned trajectories in series*/
- visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
- visual_tools.trigger();
- display_publisher.publish(display_trajectory);
-
- /* We will add more goals. But first, set the state in the planning
- scene to the final state of the last plan */
- robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
- planning_scene->setCurrentState(*robot_state.get());
-
- // Display the goal state
- visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN);
- visual_tools.publishAxisLabeled(pose.pose, "goal_2");
- visual_tools.publishText(text_pose, "Joint Space Goal (2)", rvt::WHITE, rvt::XLARGE);
- visual_tools.trigger();
-
- /* Wait for user input */
- visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
-
- /* Now, we go back to the first goal to prepare for orientation constrained planning */
- req.goal_constraints.clear();
- req.goal_constraints.push_back(pose_goal);
- context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
- context->solve(res);
- res.getMessage(response);
-
- display_trajectory.trajectory.push_back(response.trajectory);
- visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
- visual_tools.trigger();
- display_publisher.publish(display_trajectory);
-
- /* Set the state in the planning scene to the final state of the last plan */
- robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
- planning_scene->setCurrentState(*robot_state.get());
-
- // Display the goal state
- visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN);
- visual_tools.trigger();
-
- /* Wait for user input */
- visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
-
- // Adding Path Constraints
- // ^^^^^^^^^^^^^^^^^^^^^^^
- // Let's add a new pose goal again. This time we will also add a path constraint to the motion.
- /* Let's create a new pose goal */
-
- pose.pose.position.x = 0.32;
- pose.pose.position.y = -0.25;
- pose.pose.position.z = 0.65;
- pose.pose.orientation.w = 1.0;
- moveit_msgs::Constraints pose_goal_2 =
- kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle);
-
- /* Now, let's try to move to this new pose goal*/
- req.goal_constraints.clear();
- req.goal_constraints.push_back(pose_goal_2);
-
- /* But, let's impose a path constraint on the motion.
- Here, we are asking for the end-effector to stay level*/
- geometry_msgs::QuaternionStamped quaternion;
- quaternion.header.frame_id = "panda_link0";
- quaternion.quaternion.w = 1.0;
- req.path_constraints = kinematic_constraints::constructGoalConstraints("panda_link8", quaternion);
-
- // Imposing path constraints requires the planner to reason in the space of possible positions of the end-effector
- // (the workspace of the robot)
- // because of this, we need to specify a bound for the allowed planning volume as well;
- // Note: a default bound is automatically filled by the WorkspaceBounds request adapter (part of the OMPL pipeline,
- // but that is not being used in this example).
- // We use a bound that definitely includes the reachable space for the arm. This is fine because sampling is not done
- // in this volume
- // when planning for the arm; the bounds are only used to determine if the sampled configurations are valid.
- req.workspace_parameters.min_corner.x = req.workspace_parameters.min_corner.y =
- req.workspace_parameters.min_corner.z = -2.0;
- req.workspace_parameters.max_corner.x = req.workspace_parameters.max_corner.y =
- req.workspace_parameters.max_corner.z = 2.0;
-
- // Call the planner and visualize all the plans created so far.
- context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
- context->solve(res);
- res.getMessage(response);
- display_trajectory.trajectory.push_back(response.trajectory);
- visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
- visual_tools.trigger();
- display_publisher.publish(display_trajectory);
-
- /* Set the state in the planning scene to the final state of the last plan */
- robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
- planning_scene->setCurrentState(*robot_state.get());
-
- // Display the goal state
- visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN);
- visual_tools.publishAxisLabeled(pose.pose, "goal_3");
- visual_tools.publishText(text_pose, "Orientation Constrained Motion Plan (3)", rvt::WHITE, rvt::XLARGE);
- visual_tools.trigger();
-
- // END_TUTORIAL
- /* Wait for user input */
- visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to exit the demo");
- planner_instance.reset();
-
- return 0;
-}
diff --git a/src/tutorial/move_group_interface_tutorial.cpp b/src/tutorial/move_group_interface_tutorial.cpp
deleted file mode 100644
index c61ef2c42fc984f1329340ea4a18c951088e58b6..0000000000000000000000000000000000000000
--- a/src/tutorial/move_group_interface_tutorial.cpp
+++ /dev/null
@@ -1,409 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2013, SRI International
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of SRI International nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-/* Author: Sachin Chitta, Dave Coleman, Mike Lautman */
-
-#include <moveit/move_group_interface/move_group_interface.h>
-#include <moveit/planning_scene_interface/planning_scene_interface.h>
-
-#include <moveit_msgs/DisplayRobotState.h>
-#include <moveit_msgs/DisplayTrajectory.h>
-
-#include <moveit_msgs/AttachedCollisionObject.h>
-#include <moveit_msgs/CollisionObject.h>
-
-#include <moveit_visual_tools/moveit_visual_tools.h>
-
-int main(int argc, char** argv)
-{
- ros::init(argc, argv, "move_group_interface_tutorial");
- ros::NodeHandle node_handle;
- ros::AsyncSpinner spinner(1);
- spinner.start();
-
- // BEGIN_TUTORIAL
- //
- // Setup
- // ^^^^^
- //
- // MoveIt operates on sets of joints called "planning groups" and stores them in an object called
- // the `JointModelGroup`. Throughout MoveIt the terms "planning group" and "joint model group"
- // are used interchangably.
- static const std::string PLANNING_GROUP = "panda_arm";
-
- // The :planning_interface:`MoveGroupInterface` class can be easily
- // setup using just the name of the planning group you would like to control and plan for.
- moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
-
- // We will use the :planning_interface:`PlanningSceneInterface`
- // class to add and remove collision objects in our "virtual world" scene
- moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
-
- // Raw pointers are frequently used to refer to the planning group for improved performance.
- const robot_state::JointModelGroup* joint_model_group =
- move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
-
- // Visualization
- // ^^^^^^^^^^^^^
- //
- // The package MoveItVisualTools provides many capabilities for visualizing objects, robots,
- // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script.
- namespace rvt = rviz_visual_tools;
- moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
- visual_tools.deleteAllMarkers();
-
- // Remote control is an introspection tool that allows users to step through a high level script
- // via buttons and keyboard shortcuts in RViz
- visual_tools.loadRemoteControl();
-
- // RViz provides many types of markers, in this demo we will use text, cylinders, and spheres
- Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
- text_pose.translation().z() = 1.75;
- visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE);
-
- // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
- visual_tools.trigger();
-
- // Getting Basic Information
- // ^^^^^^^^^^^^^^^^^^^^^^^^^
- //
- // We can print the name of the reference frame for this robot.
- ROS_INFO_NAMED("tutorial", "Planning frame: %s", move_group.getPlanningFrame().c_str());
-
- // We can also print the name of the end-effector link for this group.
- ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str());
-
- // We can get a list of all the groups in the robot:
- ROS_INFO_NAMED("tutorial", "Available Planning Groups:");
- std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
- std::ostream_iterator<std::string>(std::cout, ", "));
-
- // Start the demo
- // ^^^^^^^^^^^^^^^^^^^^^^^^^
- visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");
-
- // Planning to a Pose goal
- // ^^^^^^^^^^^^^^^^^^^^^^^
- // We can plan a motion for this group to a desired pose for the
- // end-effector.
- geometry_msgs::Pose target_pose1;
- target_pose1.orientation.w = 1.0;
- target_pose1.position.x = 0.28;
- target_pose1.position.y = -0.2;
- target_pose1.position.z = 0.5;
- move_group.setPoseTarget(target_pose1);
-
- // Now, we call the planner to compute the plan and visualize it.
- // Note that we are just planning, not asking move_group
- // to actually move the robot.
- moveit::planning_interface::MoveGroupInterface::Plan my_plan;
-
- bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
-
- ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
-
- // Visualizing plans
- // ^^^^^^^^^^^^^^^^^
- // We can also visualize the plan as a line with markers in RViz.
- ROS_INFO_NAMED("tutorial", "Visualizing plan 1 as trajectory line");
- visual_tools.publishAxisLabeled(target_pose1, "pose1");
- visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE);
- visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
- visual_tools.trigger();
- visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
-
- // Moving to a pose goal
- // ^^^^^^^^^^^^^^^^^^^^^
- //
- // Moving to a pose goal is similar to the step above
- // except we now use the move() function. Note that
- // the pose goal we had set earlier is still active
- // and so the robot will try to move to that goal. We will
- // not use that function in this tutorial since it is
- // a blocking function and requires a controller to be active
- // and report success on execution of a trajectory.
-
- /* Uncomment below line when working with a real robot */
- move_group.move();
-
- // Planning to a joint-space goal
- // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
- //
- // Let's set a joint space goal and move towards it. This will replace the
- // pose target we set above.
- //
- // To start, we'll create an pointer that references the current robot's state.
- // RobotState is the object that contains all the current position/velocity/acceleration data.
- moveit::core::RobotStatePtr current_state = move_group.getCurrentState();
- //
- // Next get the current set of joint values for the group.
- std::vector<double> joint_group_positions;
- current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);
-
- // Now, let's modify one of the joints, plan to the new joint space goal and visualize the plan.
- joint_group_positions[0] = -1.0; // radians
- move_group.setJointValueTarget(joint_group_positions);
-
- // We lower the allowed maximum velocity and acceleration to 5% of their maximum.
- // The default values are 10% (0.1).
- // Set your preferred defaults in the joint_limits.yaml file of your robot's moveit_config
- // or set explicit factors in your code if you need your robot to move faster.
- move_group.setMaxVelocityScalingFactor(0.05);
- move_group.setMaxAccelerationScalingFactor(0.05);
-
- success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
- ROS_INFO_NAMED("tutorial", "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED");
-
- // Visualize the plan in RViz
- visual_tools.deleteAllMarkers();
- visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, rvt::XLARGE);
- visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
- visual_tools.trigger();
- visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
-
- // Planning with Path Constraints
- // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
- //
- // Path constraints can easily be specified for a link on the robot.
- // Let's specify a path constraint and a pose goal for our group.
- // First define the path constraint.
- moveit_msgs::OrientationConstraint ocm;
- ocm.link_name = "panda_link7";
- ocm.header.frame_id = "panda_link0";
- ocm.orientation.w = 1.0;
- ocm.absolute_x_axis_tolerance = 0.1;
- ocm.absolute_y_axis_tolerance = 0.1;
- ocm.absolute_z_axis_tolerance = 0.1;
- ocm.weight = 1.0;
-
- // Now, set it as the path constraint for the group.
- moveit_msgs::Constraints test_constraints;
- test_constraints.orientation_constraints.push_back(ocm);
- move_group.setPathConstraints(test_constraints);
-
- // We will reuse the old goal that we had and plan to it.
- // Note that this will only work if the current state already
- // satisfies the path constraints. So we need to set the start
- // state to a new pose.
- robot_state::RobotState start_state(*move_group.getCurrentState());
- geometry_msgs::Pose start_pose2;
- start_pose2.orientation.w = 1.0;
- start_pose2.position.x = 0.55;
- start_pose2.position.y = -0.05;
- start_pose2.position.z = 0.8;
- start_state.setFromIK(joint_model_group, start_pose2);
- move_group.setStartState(start_state);
-
- // Now we will plan to the earlier pose target from the new
- // start state that we have just created.
- move_group.setPoseTarget(target_pose1);
-
- // Planning with constraints can be slow because every sample must call an inverse kinematics solver.
- // Lets increase the planning time from the default 5 seconds to be sure the planner has enough time to succeed.
- move_group.setPlanningTime(10.0);
-
- success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
-
- move_group.move();
-
- ROS_INFO_NAMED("tutorial", "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED");
-
- // Visualize the plan in RViz
- visual_tools.deleteAllMarkers();
- visual_tools.publishAxisLabeled(start_pose2, "start");
- visual_tools.publishAxisLabeled(target_pose1, "goal");
- visual_tools.publishText(text_pose, "Constrained Goal", rvt::WHITE, rvt::XLARGE);
- visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
- visual_tools.trigger();
- visual_tools.prompt("next step");
-
- // When done with the path constraint be sure to clear it.
- move_group.clearPathConstraints();
-
- // Cartesian Paths
- // ^^^^^^^^^^^^^^^
- // You can plan a Cartesian path directly by specifying a list of waypoints
- // for the end-effector to go through. Note that we are starting
- // from the new start state above. The initial pose (start state) does not
- // need to be added to the waypoint list but adding it can help with visualizations
- std::vector<geometry_msgs::Pose> waypoints;
- waypoints.push_back(start_pose2);
-
- geometry_msgs::Pose target_pose3 = start_pose2;
-
- target_pose3.position.z -= 0.2;
- waypoints.push_back(target_pose3); // down
-
- target_pose3.position.y -= 0.2;
- waypoints.push_back(target_pose3); // right
-
- target_pose3.position.z += 0.2;
- target_pose3.position.y += 0.2;
- target_pose3.position.x -= 0.2;
- waypoints.push_back(target_pose3); // up and left
-
- // We want the Cartesian path to be interpolated at a resolution of 1 cm
- // which is why we will specify 0.01 as the max step in Cartesian
- // translation. We will specify the jump threshold as 0.0, effectively disabling it.
- // Warning - disabling the jump threshold while operating real hardware can cause
- // large unpredictable motions of redundant joints and could be a safety issue
- moveit_msgs::RobotTrajectory trajectory;
- const double jump_threshold = 0.0;
- const double eef_step = 0.01;
- double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
- ROS_INFO_NAMED("tutorial", "Visualizing plan 4 (Cartesian path) (%.2f%% acheived)", fraction * 100.0);
-
- // Visualize the plan in RViz
- visual_tools.deleteAllMarkers();
- visual_tools.publishText(text_pose, "Cartesian Path", rvt::WHITE, rvt::XLARGE);
- visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL);
- for (std::size_t i = 0; i < waypoints.size(); ++i)
- visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL);
- visual_tools.trigger();
- visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
-
- // Cartesian motions should often be slow, e.g. when approaching objects. The speed of cartesian
- // plans cannot currently be set through the maxVelocityScalingFactor, but requires you to time
- // the trajectory manually, as described [here](https://groups.google.com/forum/#!topic/moveit-users/MOoFxy2exT4).
- // Pull requests are welcome.
-
- // You can execute a trajectory by wrapping it in a plan like this.
- moveit::planning_interface::MoveGroupInterface::Plan cartesian_plan;
- cartesian_plan.trajectory_ = trajectory;
- move_group.execute(cartesian_plan);
-
- // Adding/Removing Objects and Attaching/Detaching Objects
- // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
- //
- // Define a collision object ROS message.
- moveit_msgs::CollisionObject collision_object;
- collision_object.header.frame_id = move_group.getPlanningFrame();
-
- // The id of the object is used to identify it.
- collision_object.id = "box1";
-
- // Define a box to add to the world.
- shape_msgs::SolidPrimitive primitive;
- primitive.type = primitive.BOX;
- primitive.dimensions.resize(3);
- primitive.dimensions[0] = 0.4;
- primitive.dimensions[1] = 0.1;
- primitive.dimensions[2] = 0.4;
-
- // Define a pose for the box (specified relative to frame_id)
- geometry_msgs::Pose box_pose;
- box_pose.orientation.w = 1.0;
- box_pose.position.x = 0.4;
- box_pose.position.y = -0.2;
- box_pose.position.z = 1.0;
-
- collision_object.primitives.push_back(primitive);
- collision_object.primitive_poses.push_back(box_pose);
- collision_object.operation = collision_object.ADD;
-
- std::vector<moveit_msgs::CollisionObject> collision_objects;
- collision_objects.push_back(collision_object);
-
- // Now, let's add the collision object into the world
- ROS_INFO_NAMED("tutorial", "Add an object into the world");
- planning_scene_interface.addCollisionObjects(collision_objects);
-
- // Show text in RViz of status
- visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);
- visual_tools.trigger();
-
- // Wait for MoveGroup to receive and process the collision object message
- visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");
-
- // Now when we plan a trajectory it will avoid the obstacle
- move_group.setStartState(*move_group.getCurrentState());
- geometry_msgs::Pose another_pose;
- another_pose.orientation.w = 1.0;
- another_pose.position.x = 0.4;
- another_pose.position.y = -0.4;
- another_pose.position.z = 0.9;
- move_group.setPoseTarget(another_pose);
-
- success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
- ROS_INFO_NAMED("tutorial", "Visualizing plan 5 (pose goal move around cuboid) %s", success ? "" : "FAILED");
-
- // Visualize the plan in RViz
- visual_tools.deleteAllMarkers();
- visual_tools.publishText(text_pose, "Obstacle Goal", rvt::WHITE, rvt::XLARGE);
- visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
- visual_tools.trigger();
- visual_tools.prompt("next step");
-
- // Now, let's attach the collision object to the robot.
- ROS_INFO_NAMED("tutorial", "Attach the object to the robot");
- move_group.attachObject(collision_object.id);
-
- // Show text in RViz of status
- visual_tools.publishText(text_pose, "Object attached to robot", rvt::WHITE, rvt::XLARGE);
- visual_tools.trigger();
-
- /* Wait for MoveGroup to receive and process the attached collision object message */
- visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object attaches to the "
- "robot");
-
- // Now, let's detach the collision object from the robot.
- ROS_INFO_NAMED("tutorial", "Detach the object from the robot");
- move_group.detachObject(collision_object.id);
-
- // Show text in RViz of status
- visual_tools.publishText(text_pose, "Object dettached from robot", rvt::WHITE, rvt::XLARGE);
- visual_tools.trigger();
-
- /* Wait for MoveGroup to receive and process the attached collision object message */
- visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object detaches to the "
- "robot");
-
- // Now, let's remove the collision object from the world.
- ROS_INFO_NAMED("tutorial", "Remove the object from the world");
- std::vector<std::string> object_ids;
- object_ids.push_back(collision_object.id);
- planning_scene_interface.removeCollisionObjects(object_ids);
-
- // Show text in RViz of status
- visual_tools.publishText(text_pose, "Object removed", rvt::WHITE, rvt::XLARGE);
- visual_tools.trigger();
-
- /* Wait for MoveGroup to receive and process the attached collision object message */
- visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disapears");
-
- // END_TUTORIAL
-
- ros::shutdown();
- return 0;
-}
diff --git a/src/util/MqttUtil.cpp b/src/util/MqttUtil.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e0f9459fa0ccc20a67564d14e0ad6fe943a74a09
--- /dev/null
+++ b/src/util/MqttUtil.cpp
@@ -0,0 +1,59 @@
+//
+// Created by Johannes Mey on 01.06.20.
+//
+
+#include "MqttUtil.h"
+
+#include "ros/ros.h"
+
+bool MqttUtil::connect() {
+ try {
+ if (!client.is_connected()) {
+ ROS_INFO_NAMED("MqttUtil", "Initializing MQTT");
+ mqtt::connect_options connOpts;
+ connOpts.set_keep_alive_interval(20);
+ connOpts.set_clean_session(false);
+ ROS_INFO_NAMED("MqttUtil", "MQTT initialized. Connecting...");
+ auto rsp = client.connect(connOpts);
+ if (!rsp.is_session_present()) {
+ client.subscribe(topics);
+ return true;
+ }
+ ROS_INFO_NAMED("MqttUtil", "Connection established.");
+ } else {
+ ROS_WARN_NAMED("MqttUtil", "Client is already connected.");
+ return true;
+ }
+ } catch (const mqtt::exception &) { std::this_thread::sleep_for(std::chrono::seconds(1)); }
+ return false;
+}
+
+bool MqttUtil::ensureConnection() {
+ constexpr int N_ATTEMPT = 30;
+
+ if (!client.is_connected()) {
+ ROS_WARN_STREAM_NAMED("MqttUtil", "Lost connection. Reconnecting...");
+ for (int i = 0; i < N_ATTEMPT && !client.is_connected(); ++i) {
+ try {
+ auto rsp = client.reconnect();
+ if (!rsp.is_session_present()) {
+ client.subscribe(topics);
+ }
+ ROS_WARN_STREAM_NAMED("MqttUtil", "Reconnection succeeded!");
+ return true;
+ } catch (const mqtt::exception &) { std::this_thread::sleep_for(std::chrono::seconds(1)); }
+ }
+ ROS_ERROR_STREAM_NAMED("MqttUtil", "Reconnection failed!");
+ return false;
+ }
+ return true;
+}
+
+const mqtt::string_collection &MqttUtil::getTopics() const { return topics; }
+
+void MqttUtil::addTopic(std::string topic) { topics.push_back(topic); }
+
+MqttUtil::MqttUtil(std::string client_id, std::string server) : topics(), client(server, client_id) {}
+
+MqttUtil::~MqttUtil() { client.disconnect(); }
+mqtt::client &MqttUtil::getClient() { return client; }
diff --git a/src/util/MqttUtil.h b/src/util/MqttUtil.h
new file mode 100644
index 0000000000000000000000000000000000000000..81fe1749a3e0941876014e06e12bff37af9e5e1b
--- /dev/null
+++ b/src/util/MqttUtil.h
@@ -0,0 +1,32 @@
+//
+// Created by Johannes Mey on 01.06.20.
+//
+
+#ifndef PANDA_MQTT_CONNECTOR_MQTTUTIL_H
+#define PANDA_MQTT_CONNECTOR_MQTTUTIL_H
+
+#include <mqtt/async_client.h>
+#include <mqtt/client.h>
+
+const std::string DEFAULT_SERVER_ADDRESS{"tcp://localhost:1883"};
+
+class MqttUtil {
+
+private:
+ mqtt::client client;
+ mqtt::string_collection topics;
+
+public:
+ MqttUtil(std::string client_id, std::string server = DEFAULT_SERVER_ADDRESS);
+ const mqtt::string_collection &getTopics() const;
+ void addTopic(std::string topic);
+
+ mqtt::client &getClient();
+
+ bool connect();
+
+ bool ensureConnection();
+ virtual ~MqttUtil();
+};
+
+#endif // PANDA_MQTT_CONNECTOR_MQTTUTIL_H