MqttRosConnectionTestNode.cpp

//
// Created by sebastian on 31.03.20.
//
#include "config.pb.h"
#include "trajectory.pb.h"

#include "ros/ros.h"

#include "util/MqttUtil.h"

const std::string CLIENT_ID{"ros_mqtt_tester"};

MqttUtil *mqttUtil = nullptr;

void testTrajectoryUpdate(const ros::NodeHandle& n) {

  ROS_INFO_STREAM("TRAJECTORY UPDATE TEST: Sending simple two-point trajectory to planner.");

  plan::Trajectory trajectory{};
  trajectory.set_loop(false);

  plan::Trajectory_Pose* pose = trajectory.add_pose();
  pose->set_mode(plan::Trajectory_PlanningMode_FLUID);
  pose->mutable_orientation()->set_w(0);
  pose->mutable_orientation()->set_x(0.7071067811865476);
  pose->mutable_orientation()->set_y(0.7071067811865476);
  pose->mutable_orientation()->set_z(0);

  pose->mutable_position()->set_x(0.4);
  pose->mutable_position()->set_y(0.4);
  pose->mutable_position()->set_z(0.4);

  plan::Trajectory_Pose* secondPose = trajectory.add_pose();
  secondPose->CopyFrom(*pose);
  secondPose->mutable_position()->set_x(-0.4);

  std::string mqtt_msg{};
  trajectory.SerializeToString(&mqtt_msg);
  auto pubmsg = mqtt::make_message("trajectory", mqtt_msg);
  mqttUtil->getClient().publish(pubmsg);
}

void testSpeedFactorChange(const ros::NodeHandle &n) {

  std::string planning_mode;
  if (!n.getParam("robot_planning_mode", planning_mode)) {
    ROS_ERROR_STREAM("No " << n.getNamespace() << "/robot_planning_mode set. Aborting planning mode change test.");
    return;
  }

  config::RobotConfig rc;

  double motionSpeedFactor;
  if (!n.getParam("robot_speed_factor", motionSpeedFactor)) {
    ROS_ERROR_STREAM("No " << n.getNamespace() << "/robot_speed_factor set. Aborting planning mode change test.");
    return;
  }

  if (motionSpeedFactor < 0.5) {
    ROS_INFO_STREAM("SPEED FACTOR CHANGE TEST: changed from " << motionSpeedFactor << " to 0.9");
    rc.set_speed(0.9);
  } else {
    ROS_INFO_STREAM("SPEED FACTOR CHANGE TEST: changed from " << motionSpeedFactor << " to 0.2");
    rc.set_speed(0.2);
  }

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

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

void receiveMqttMessages(const ros::NodeHandle &n) {

  if (mqttUtil->ensureConnection()) {
    auto *msg = new mqtt::const_message_ptr;
    if (mqttUtil->getClient().try_consume_message_for(msg, std::chrono::milliseconds(500))) {
      ROS_INFO_STREAM("Received message on topic " << msg->get()->get_topic());
      if (msg->get()->get_topic() == "test_speed_change") {
        testSpeedFactorChange(n);
      } else if (msg->get()->get_topic() == "test_trajectory_update") {
        testTrajectoryUpdate(n);
      }
    }

  } else {
    ROS_ERROR_STREAM("Not connected! Unable to listen to messages.");
  }
}

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

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

  std::string server;
  if (!n.getParam("server", server)) {
    ROS_ERROR("Could not get string value for panda_mqtt_connector/server from param server");
    return -1;
  }

  mqttUtil = new MqttUtil(CLIENT_ID, server);

  mqttUtil->addTopic("test_speed_change");
  mqttUtil->addTopic("test_trajectory_update");

  mqttUtil->connect();

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

  return 0;
}