Update robot_models_node.cpp

src/robot_models_node.cpp

@@ -131,75 +131,15 @@
 
                 }
 
-                geometry_msgs::PoseStamped ros_msg;
-                ros_msg.header.stamp = transformStamped.header.stamp;
-                ros_msg.header.frame_id = "joint";
-
-                //ros_msg.pose.position.x = transformStamped.transform.translation.x;
-                //ros_msg.pose.position.y = transformStamped.transform.translation.y;
-                //ros_msg.pose.position.z = transformStamped.transform.translation.z;
-
-                ros_msg.pose.orientation.x = transformStamped.transform.rotation.x;
-                ros_msg.pose.orientation.y = transformStamped.transform.rotation.y;
-                ros_msg.pose.orientation.z = transformStamped.transform.rotation.z;
-                //ros_msg.pose.orientation.w = transformStamped.transform.rotation.w;
-                //ros_msg.pose.orientation.x = transformStamped.transform.rotation.x;
-                //ros_msg.pose.orientation.y = transformStamped.transform.rotation.y;
-                //ros_msg.pose.orientation.z = transformStamped.transform.rotation.z;
-                ros_msg.pose.orientation.w = transformStamped.transform.rotation.w;
-                ros_msg.pose.orientation.x = transformStamped.transform.rotation.x;
-                ros_msg.pose.orientation.y = transformStamped.transform.rotation.y;
-                ros_msg.pose.orientation.z = transformStamped.transform.rotation.z;
-
-                 //turtle_vel.publish(ros_msg);
-                //publishers[joint.second].publish(ros_msg);
-
-                // publish the MQTT message
-                robot::RobotState pls_msg;
-                //robot::RobotState pls_msg;
-                // set the name to the MQTT topic provided by the topics map
-                 pls_msg.set_name(topic.second);
-                // robot::RobotState pls_msg;
-                auto *pos = new robot::RobotState_Position();
-                pos->set_o5a.p1(transformStamped.transform.translation.x);
-                pos->set_o5b.p1(transformStamped.transform.translation.y);
-                pos->set_o5c.p1(transformStamped.transform.translation.z);
-                pos->set_o5d.p1(transformStamped.transform.translation.z);
-                pos->set_o5e.p1(transformStamped.transform.translation.z);
-                pos->set_o5f.p1(transformStamped.transform.translation.z);
-                pos->set_o5g.p1(transformStamped.transform.translation.z);
-
-                pls_msg.set_allocated_position(pos);
-
-                //auto *pos = new robot::RobotState_Position();
-                //pos->set_x(transformStamped.transform.translation.x);
-                //pos->set_y(transformStamped.transform.translation.y);
-                //pos->set_z(transformStamped.transform.translation.z);
-                //pls_msg.set_allocated_position(pos);
-
-
-                auto *orient = new robot::RobotState_Orientation();
-                orient->set_o5a.or2(transformStamped.transform.rotation.w);
-                orient->set_o5b.or2(transformStamped.transform.rotation.x);
-                orient->set_o5c.or2(transformStamped.transform.rotation.y);
-                orient->set_o5d.or2(transformStamped.transform.rotation.z);
-                orient->set_o5e.or2(transformStamped.transform.rotation.z);
-                orient->set_o5f.or2(transformStamped.transform.rotation.z);
-                orient->set_o5g.or2(transformStamped.transform.rotation.z);
-
-                pls_msg.set_allocated_orientation(orient);
-
-                //auto *orient = new robot::RobotState_Orientation();
-                //orient->set_w(transformStamped.transform.rotation.w);
-                //orient->set_x(transformStamped.transform.rotation.x);
-                //orient->set_y(transformStamped.transform.rotation.y);
-                //orient->set_z(transformStamped.transform.rotation.z);
-                //pls_msg.set_allocated_orientation(orient);
-
-
-                 //robot_models_node::sendMqttMessages(pls_msg);
-                //robot_state_provider::sendMqttMessages(pls_msg);
 
+               o5a.p1.push_back(transformStamped.transform.translation.x);
+               o5a.p1.push_back(transformStamped.transform.translation.y);
+               o5a.p1.push_back(transformStamped.transform.translation.y);
+
+               o5a.or2.w=transformStamped.transform.rotation.w;
+               o5a.or2.x=transformStamped.transform.rotation.x;
+               o5a.or2.y=transformStamped.transform.rotation.y;
+               o5a.or2.z=transformStamped.transform.rotation.z;
 
               }