fix some bugs

src/MqttRosConnectionTestNode.cpp

@@ -109,7 +109,6 @@ void testConfig(ros::NodeHandle n) {
 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") {

src/MqttToRosNode.cpp

@@ -122,7 +122,7 @@ int main(int argc, char *argv[]) {
   mqttUtil.addTopic("trajectoryconfig");
 
   if (!mqttUtil.connect()) {
-    ROS_INFO_STREAM("Unable to connect to MQTT server. Exiting...");
+    ROS_ERROR_STREAM("Unable to connect to MQTT server. Exiting...");
     return -1;
   }
 

src/RobotStateProvider.cpp

@@ -102,14 +102,18 @@ panda::PandaLinkState buildLinkStateMessage(const gazebo_msgs::LinkStates &msg,
 
   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;
+  if (!n.getParam("data_enable_position", export_position)) {
+    export_position = false;
+  }
+  if (!n.getParam("data_enable_orientation", export_orientation)) {
+    export_orientation = false;
+  }
+  if (!n.getParam("data_enable_twist_linear", export_twist_linear)) {
+    export_twist_linear = false;
+  }
+  if (!n.getParam("data_enable_twist_angular", export_twist_angular)) {
+    export_twist_angular = false;
+  }
 
   panda::PandaLinkState pls_msg;
 
@@ -160,7 +164,6 @@ void sendMqttMessages(const gazebo_msgs::LinkStates &msg) {
         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);
@@ -182,7 +185,9 @@ void providerCallback(const gazebo_msgs::LinkStates &msg) {
 
   ros::NodeHandle n;
   int message_redirect_rate;
-  n.getParam("data_publish_rate", message_redirect_rate);
+  if (!n.getParam("data_publish_rate", message_redirect_rate)) {
+    message_redirect_rate = 200; // fallback
+  }
 
   if (current_redirect == 0) {
     if (export_to_log) {
@@ -190,27 +195,17 @@ void providerCallback(const gazebo_msgs::LinkStates &msg) {
     }
     sendMqttMessages(msg);
   }
-  current_redirect = ++current_redirect % message_redirect_rate;
+  current_redirect = (current_redirect + 1) % message_redirect_rate;
 }
 
 int main(int argc, char **argv) {
   ros::init(argc, argv, "robot_state_provider");
   ros::NodeHandle n;
-  n.getParam("ready", isInitialized);
 
   mqttUtil.connect();
 
-  while (ros::ok()) {
-
-    // make sure the robot is initialized
-    while (!isInitialized) {
-      // std::cout << "Waiting" << std::endl;
-      n.getParam("tud_planner_ready", isInitialized);
-    }
-
   ros::Subscriber sub = n.subscribe("/gazebo/link_states", 10000, providerCallback);
   ros::spin();
-  }
 
   return 0;
 }

src/TimedPlanner.cpp

@@ -98,11 +98,11 @@ void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::Mo
 
     for (int i = 0; i < PlannerState::raw_trajectory.size(); i++) {
 
-        bool newTrajectoryAvaible = false;
-        node_handle.getParam("new_trajectory_available", newTrajectoryAvaible);
+        bool newTrajectoryAvailable = false;
+        node_handle.getParam("new_trajectory_available", newTrajectoryAvailable);
         node_handle.getParam("loop_trajectory", PlannerState::isLooping);
 
-        if (newTrajectoryAvaible) {
+        if (newTrajectoryAvailable) {
             if (!updateRawTrajectory(node_handle, &group)) {
                 break;
             }
@@ -111,7 +111,7 @@ void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::Mo
 
         // wake up if we have trajectory
         while (PlannerState::raw_trajectory.size() == 0) {
-            if (newTrajectoryAvaible) {
+            if (newTrajectoryAvailable) {
                 updateRawTrajectory(node_handle, &group);
                 i = 0;
                 break;

src/util/MqttUtil.cpp

@@ -13,33 +13,33 @@ bool MqttUtil::connect() {
       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.");
+      return true;
     } else {
       ROS_WARN_NAMED("MqttUtil", "Client is already connected.");
       return true;
     }
-  } catch (const mqtt::exception &) { std::this_thread::sleep_for(std::chrono::seconds(1)); }
+  } catch (const mqtt::exception &e) {
+    ROS_ERROR_STREAM("Unable to connect because of exception " << e.get_message());
     return false;
   }
+}
 
 bool MqttUtil::ensureConnection() {
   constexpr int N_ATTEMPT = 30;
 
   if (!client.is_connected()) {
-    ROS_WARN_STREAM_NAMED("MqttUtil", "Lost connection. Reconnecting...");
+    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)); }
     }