MqttRosConnectionTestNode.cpp

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

using namespace std;

/*
 * mqtt-topics for all links
 * ground, link0, link1, link2, link3, link4, link5, link6, link7 (end-effector), left finger, right finder
 */
std::string topics[11] = {"panda_ground", "panda_link_0", "panda_link_1", "panda_link_2",
                          "panda_link_3", "panda_link_4", "panda_link_5", "panda_link_6",
                          "panda_link_7", "panda_link_8", "panda_link_9"};

const string CLIENT_ID{"ros_mqtt_tester"};

MqttUtil mqttUtil(CLIENT_ID);

void testTrajectoryUpdate(ros::NodeHandle n) {

  ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", ">>>>>>> STARTING TRAJECTORY UPDATE TEST <<<<<<<");

  plan::Trajectory traj;

  plan::Trajectory_Position pos1;
  pos1.set_x(0.6);
  pos1.set_y(0.0);
  pos1.set_z(0.6);

  traj.add_pos()->CopyFrom(pos1);

  plan::Trajectory_Position pos2;
  pos2.set_x(-0.6);
  pos2.set_y(0.0);
  pos2.set_z(0.6);

  traj.add_pos()->CopyFrom(pos2);

  std::string mqtt_msg;
  traj.SerializeToString(&mqtt_msg);

  auto pubmsg = mqtt::make_message("trajectoryconfig", mqtt_msg);
  mqttUtil.getClient().publish(pubmsg);

  ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", ">>>>>>>>>>>> CHECKING SERVER <<<<<<<<<<<<<");

  std::vector<double> x_pos;
  std::vector<double> y_pos;
  std::vector<double> z_pos;

  ros::Duration(5.0).sleep();

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

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

  ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", ">>>>>>> STARTING PLANNING MODE CHANGE TEST <<<<<<<");

  config::RobotConfig rc;

  rc.set_speed(1.0);
  rc.set_planningmode(config::RobotConfig_PlanningMode_FLUID);

  std::string mqtt_msg;
  rc.SerializeToString(&mqtt_msg);

  auto pubmsg = mqtt::make_message("robotconfig", mqtt_msg);
  mqttUtil.getClient().publish(pubmsg);
}

void testConfig(ros::NodeHandle n) {

  ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", ">>>>>>> STARTING CONFIG UPDATE TEST <<<<<<<");

  config::DataConfig dc;

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

  auto pubmsg = mqtt::make_message("dataconfig", mqtt_msg);
  mqttUtil.getClient().publish(pubmsg);
}

void receiveMqttMessages(ros::NodeHandle n) {

  if (mqttUtil.ensureConnection()) {
    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);
      }
    }

  } else {
    ROS_ERROR_STREAM_NAMED("MqttRosConnectionTestNode", "Not connected! Unable to listen to messages.");
  }
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "MqttRosConnectionTestNode");
  ros::NodeHandle n;

  ROS_INFO_NAMED("MqttRosConnectionTestNode", "Setting up MQTT.");

  mqttUtil.addTopic("test_config");
  mqttUtil.addTopic("test_mode_change");
  mqttUtil.addTopic("test_trajectory_update");

  mqttUtil.connect();

  while (ros::ok()) {
    receiveMqttMessages(n);
    ros::spinOnce();
  }

  return 0;
}