From 16eed0ad4e899530c6043a8a9242295a19e5a539 Mon Sep 17 00:00:00 2001 From: Emanuele Date: Fri, 15 Nov 2019 12:22:50 +0100 Subject: [PATCH] Inizializzazione broadcaster con operazioni standard dei nodi --- src/roboglue_ros/src/roboglue_broadcaster.cpp | 127 ++++++++++++++++-- 1 file changed, 115 insertions(+), 12 deletions(-) diff --git a/src/roboglue_ros/src/roboglue_broadcaster.cpp b/src/roboglue_ros/src/roboglue_broadcaster.cpp index 6b6dee9..32239dd 100644 --- a/src/roboglue_ros/src/roboglue_broadcaster.cpp +++ b/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 /// 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("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); - while (ros::ok()) - { - std_msgs::String msg; - msg.data = "hello world"; + /// subscribe to incoming topics /// + ros::Subscriber command_sub = nh.subscribe(common_par.ros_topics.commandSub, static_cast(common_par.msgBufferSize), + boost::bind(commandCallback, _1, &is, &publishers, &pos_data, &robot_data)); + ros::Subscriber state_sub = nh.subscribe(common_par.ros_topics.stateSub, static_cast(common_par.msgBufferSize), + boost::bind(stateCallback, _1, &is, &aux_data)); + /// advertise publish topics // + ros::Publisher command_pub = nh.advertise(common_par.ros_topics.commandPub, + static_cast(common_par.msgBufferSize)); + ros::Publisher interface_pub = nh.advertise(common_par.ros_topics.interfacePub, + static_cast(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_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 ///////////////// +////////////////////////////////////////