Inizializzazione broadcaster con operazioni standard dei nodi

src/roboglue_ros/src/roboglue_broadcaster.cpp

@@ -1,30 +1,133 @@
 /*
-20190220 - Inizia la scrittura del nodo che si occupa di pubblicate le trasformazioni TF, documentazione su GIT
+20191115 - Inizia la scrittura del nodo che si occupa di pubblicate le trasformazioni TF, documentazione su GIT
 */
 
 ////////// ROS INCLUDES /////////////
 #include "ros/ros.h"
 #include <roboglue_ros/roboglue_utils.h> /// utility function library
 
-int main(int argc, char **argv)
+///////////////////////////////////////////
-{
+///////// ROS CALLBACKS //////////////////
+/////////////////////////////////////////
+void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, publisherMap* pubs, positionData* pos, robotData* rdata) {
+    ROS_DEBUG("Received command: [%s]", msg->data.c_str());
+    try {
+        nlohmann::json c = nlohmann::json::parse(msg->data.c_str());
+        std::string command = c["command"];
+        nlohmann::json params = c["params"];
+        if (!command.compare("QUIT")){
+            is->isRunning = false;
+        } else {
+            ROS_WARN("Unknown command: %s", command.c_str());
+        }
+    } catch (nlohmann::json::exception &e){
+        ROS_ERROR("Failed to parse COMMAND: [%s]", e.what());
+    }
+}
+
+void stateCallback(const std_msgs::String::ConstPtr& msg, internalState* is, auxData* aux){
+    ROS_DEBUG("Received aux I/O State Update: [%s]", msg->data.c_str());
+}
+
+void interfaceCallback(const std_msgs::String::ConstPtr& msg, internalState *is){
+    ROS_DEBUG("Received Interface Message: [%s]", msg->data.c_str());
+}
+
+void heartBeatCallback(const ros::TimerEvent& t, publisherMap* p){
+    try {
+        std_msgs::String hbMsg;
+        hbMsg.data = std::string("HB:"+
+                                 boost::replace_all_copy(ros::this_node::getName(), "/", "")+
+                                 ":" + std::to_string(t.current_real.toSec()));
+        p->find("/roboglue_com/ros2com/interface")->second->publish(hbMsg);
+    } catch (std::exception &e) {
+        ROS_ERROR("Impossible to Send HeartBeat -> %s", e.what());
+    }
+}
+
+////////////////////////////////////////
+//////////////// MAIN //////////////////
+////////////////////////////////////////
+int main(int argc, char **argv) {
+    /// node variables ///
+    COMMON_parameters common_par;
+    /// internal state struct ////
+    internalState is;
+    positionData pos_data;
+    robotData robot_data;
+    auxData aux_data;
+    int tfPubRate;
+
+     /// setup node parameters ///
     ros::init(argc, argv, "roboglue_broadcaster");
     ros::NodeHandle nh;
+    publisherMap publishers;
+    /// TODO: override default signal handler ///
+    // signal(SIGINT, boost::bind(sigintHandler, _1, &is));
+    ///read parameters from server ///
+    nh.getParam("/roboglue_ros_broadcaster/parameter_ns", common_par.param_ns);
+    loadCommonParameters(&nh, &common_par);
+    // load robot model information
+    nh.getParam(common_par.param_ns+"robot_model_name", robot_data.robot_model_name);
+    nh.getParam(common_par.param_ns+"move_group_name", robot_data.move_group_name);
+    nh.getParam(common_par.param_ns+"jointHomePosition", robot_data.joint_homes);
+    // load jog coord publish rate
+    nh.getParam("/roboglue_ros_broadcaster/tf_pub_rate", tfPubRate);
 
-    ros::Publisher chatter_pub = nh.advertise<std_msgs::String>("chatter", 1000);
+    /// set spinner rate ///
+    ros::Rate loop_rate(common_par.loopRate);
+    /// set console log level ///
+    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
+    ROS_INFO("Follower Node Started");
 
-    ros::Rate loop_rate(10);
+    /// subscribe to incoming topics ///
-    while (ros::ok())
+    ros::Subscriber command_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.commandSub, static_cast<uint32_t>(common_par.msgBufferSize),
-    {
+                                                                 boost::bind(commandCallback, _1, &is, &publishers, &pos_data, &robot_data));
-        std_msgs::String msg;
+    ros::Subscriber state_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.stateSub, static_cast<uint32_t>(common_par.msgBufferSize),
-        msg.data = "hello world";
+                                                               boost::bind(stateCallback, _1, &is, &aux_data));
+    /// advertise publish topics //
+    ros::Publisher command_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.commandPub,
+                                                                static_cast<uint32_t>(common_par.msgBufferSize));
+    ros::Publisher interface_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.interfacePub,
+                                                              static_cast<uint32_t>(common_par.msgBufferSize));
 
-        chatter_pub.publish(msg);
+    /// build a list of publisher to pass to mqtt callback ///
+    publishers[command_pub.getTopic()] = &command_pub;
+    publishers[interface_pub.getTopic()] = &interface_pub;
 
+    /// load the robot model for inverse kinematics and self collision checking ///
+    /// requires parameter server and moveIT to be active
+    moveit::planning_interface::MoveGroupInterface move_group(robot_data.move_group_name);
+    robot_data.kine_model = move_group.getRobotModel();
+    robot_data.kine_state = std::make_shared<robot_state::RobotState>(robot_data.kine_model);
+    robot_data.joint_modelGroup = robot_data.kine_state->getJointModelGroup(robot_data.move_group_name);
+    robot_data.joint_names = robot_data.joint_modelGroup->getVariableNames();
+    ROS_INFO("Planning in Frame: %s for link: %s", move_group.getPlanningFrame().c_str(), move_group.getEndEffectorLink().c_str());
+    ROS_INFO("IK calc in Frame: %s", robot_data.kine_model->getModelFrame().c_str());
+
+    /// create timed callbacks ///
+    ros::Timer heartBeat = nh.createTimer(ros::Duration(5.0),
+                                          boost::bind(heartBeatCallback, _1, &publishers));
+    ///////////////////// MAIN LOOP //////////////////////
+    bool startCycle = true;
+    while (ros::ok() && is.isRunning) {
+        if (startCycle){
+            startCycle = false;
+            declareStartup(&publishers, &common_par);
+            ros::spinOnce();
+        }
+        ROS_DEBUG_ONCE("Broadcaster Node Looping");
         ros::spinOnce();
-
+        if(ros::isShuttingDown()){
+            declareShutdown(&publishers, &common_par);
+            ros::spinOnce();
+        }
         loop_rate.sleep();
     }
-
+    ros::shutdown();
+    //////////////////////////////////////////////////////
     return 0;
 }
+////////////////////////////////////////
+///////////// END MAIN /////////////////
+////////////////////////////////////////