From 8ff1c7e84ad3020b53e5be2d9a9644d2e289b19b Mon Sep 17 00:00:00 2001 From: Emanuele Date: Tue, 29 Oct 2019 11:47:18 +0100 Subject: [PATCH] Weaponizzata la logica che carica i parametri da ROSparam Raggruppati tutti i parametri comuni in una struttura uguale per tutti i nodi. Aggiungere altri parametri alla lista dei comuni --- .../include/roboglue_ros/roboglue_utils.h | 10 +++ src/roboglue_ros/src/roboglue_follower.cpp | 61 ++++++++----------- src/roboglue_ros/src/roboglue_recorder.cpp | 58 ++++++++---------- src/roboglue_ros/src/roboglue_utils.cpp | 13 ++++ 4 files changed, 76 insertions(+), 66 deletions(-) diff --git a/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h b/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h index 67ed80e..8f52fd2 100644 --- a/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h +++ b/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h @@ -47,6 +47,13 @@ typedef struct { std::string delta_jog, delta_joint_jog, traj_jog; } ROS_topics; +///////// COMMON PARAMETERS ////////////// +typedef struct { + ROS_topics ros_topics; + int loopRate, msgBufferSize; + std::string param_ns; +} COMMON_parameters; + ///////// AUX DATA STORAGE /////////////// typedef struct { std::list> digital; @@ -105,6 +112,9 @@ typedef struct { mvt::MoveItVisualToolsPtr vtools; } rvizData; +///////// LOAD COMMON ROS TOPICS AND PARAMETERS ///////////////////// +bool loadCommonParameters(ros::NodeHandle*, COMMON_parameters*); + geometry_msgs::TwistStamped jstr2Twist(nlohmann::json); trajectory_msgs::JointTrajectory jstr2Joint(nlohmann::json); diff --git a/src/roboglue_ros/src/roboglue_follower.cpp b/src/roboglue_ros/src/roboglue_follower.cpp index c0e1dd7..5090472 100644 --- a/src/roboglue_ros/src/roboglue_follower.cpp +++ b/src/roboglue_ros/src/roboglue_follower.cpp @@ -176,60 +176,51 @@ void timedPublishCallback(const ros::TimerEvent&, internalState* is, publisherMa //////////////////////////////////////// int main(int argc, char **argv) { /// node variables /// - int loopRate, msgBufferSize, jogPubRate; - std::string param_ns; + COMMON_parameters common_par; std::string startup_msg_tmp; std_msgs::String startup_msg; - ROS_topics ros_topics; /// internal state struct //// internalState is; positionData pos_data; robotData robot_data; auxData aux_data; std::vector joint_names; + int jogPubRate; /// setup node parameters /// ros::init(argc, argv, "roboglue_follower"); ros::NodeHandle nh; ///read parameters from server /// - nh.getParam("/roboglue_ros_relay/parameter_ns", param_ns); - nh.getParam(param_ns+"startup_msg", startup_msg_tmp); - nh.getParam(param_ns+"loop_rate", loopRate); - nh.getParam(param_ns+"msg_buffer", msgBufferSize); - nh.getParam(param_ns+"INstate", ros_topics.stateSub); - nh.getParam(param_ns+"INcommands", ros_topics.commandSub); - nh.getParam(param_ns+"INcoordinates", ros_topics.coordSub); - nh.getParam(param_ns+"OUTstate", ros_topics.statePub); - nh.getParam(param_ns+"OUTcommands", ros_topics.commandPub); - nh.getParam(param_ns+"OUTcoordinates", ros_topics.coordPub); - nh.getParam(param_ns+"OUTdelta_jog", ros_topics.delta_jog); - nh.getParam(param_ns+"OUTdelta_joint_jog", ros_topics.delta_joint_jog); - nh.getParam(param_ns+"OUTtraj_jog", ros_topics.traj_jog); - nh.getParam(param_ns+"robot_model_name", robot_data.robot_model_name); - nh.getParam(param_ns+"move_group_name", robot_data.move_group_name); - nh.getParam(param_ns+"jointHomePosition", robot_data.joint_homes); + nh.getParam("/roboglue_ros_recorder/parameter_ns", common_par.param_ns); + loadCommonParameters(&nh, &common_par); + nh.getParam(common_par.param_ns+"OUTdelta_jog", common_par.ros_topics.delta_jog); + nh.getParam(common_par.param_ns+"OUTdelta_joint_jog", common_par.ros_topics.delta_joint_jog); + nh.getParam(common_par.param_ns+"OUTtraj_jog", common_par.ros_topics.traj_jog); + 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); nh.getParam("/roboglue_ros_follower/jog_pub_rate", jogPubRate); /// set spinner rate /// - ros::Rate loop_rate(loopRate); + 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"); /// advertise publish topics /// publisherMap publishers; - ros::Publisher jog_pub = nh.advertise(ros_topics.delta_jog, - static_cast(msgBufferSize)); - ros::Publisher jog_joint_pub = nh.advertise(ros_topics.delta_joint_jog, - static_cast(msgBufferSize)); - ros::Publisher trj_pub = nh.advertise(ros_topics.traj_jog, - static_cast(msgBufferSize)); - ros::Publisher command_pub = nh.advertise(ros_topics.commandPub, - static_cast(msgBufferSize)); - ros::Publisher coord_pub = nh.advertise(ros_topics.coordPub, - static_cast(msgBufferSize)); - ros::Publisher state_pub = nh.advertise(ros_topics.statePub, - static_cast(msgBufferSize)); + ros::Publisher jog_pub = nh.advertise(common_par.ros_topics.delta_jog, + static_cast(common_par.msgBufferSize)); + ros::Publisher jog_joint_pub = nh.advertise(common_par.ros_topics.delta_joint_jog, + static_cast(common_par.msgBufferSize)); + ros::Publisher trj_pub = nh.advertise(common_par.ros_topics.traj_jog, + static_cast(common_par.msgBufferSize)); + ros::Publisher command_pub = nh.advertise(common_par.ros_topics.commandPub, + static_cast(common_par.msgBufferSize)); + ros::Publisher coord_pub = nh.advertise(common_par.ros_topics.coordPub, + static_cast(common_par.msgBufferSize)); + ros::Publisher state_pub = nh.advertise(common_par.ros_topics.statePub, + static_cast(common_par.msgBufferSize)); /// build a list of publisher to pass to mqtt callback /// publishers[command_pub.getTopic()] = &command_pub; @@ -241,11 +232,11 @@ int main(int argc, char **argv) { publishers["trj_pub"] = &trj_pub; /// subscribe to incoming topics /// - ros::Subscriber command_sub = nh.subscribe(ros_topics.commandSub, static_cast(msgBufferSize), + 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 coordinate_sub = nh.subscribe(ros_topics.coordSub, static_cast(msgBufferSize), + ros::Subscriber coordinate_sub = nh.subscribe(common_par.ros_topics.coordSub, static_cast(common_par.msgBufferSize), boost::bind(coordinateCallback, _1, &publishers, &pos_data)); - ros::Subscriber state_sub = nh.subscribe(ros_topics.stateSub, static_cast(msgBufferSize), + ros::Subscriber state_sub = nh.subscribe(common_par.ros_topics.stateSub, static_cast(common_par.msgBufferSize), boost::bind(stateCallback, _1, &is, &aux_data)); /// load the robot model for inverse kinematics and self collision checking /// /// requires parameter server and moveIT to be active diff --git a/src/roboglue_ros/src/roboglue_recorder.cpp b/src/roboglue_ros/src/roboglue_recorder.cpp index d7fb189..bc12a72 100644 --- a/src/roboglue_ros/src/roboglue_recorder.cpp +++ b/src/roboglue_ros/src/roboglue_recorder.cpp @@ -204,15 +204,16 @@ void stateCallback(const std_msgs::String::ConstPtr& msg, internalState*is, auxD } } +void interfaceCallback(const std_msgs::String::ConstPtr& msg, internalState *is){ + ROS_DEBUG("Received Interface Message; [%s]", msg->data.c_str()); +} + //////////////////////////////////////// //////////////// MAIN ////////////////// //////////////////////////////////////// int main(int argc, char **argv) { /// node variables //// - int loopRate, msgBufferSize; - double nonzero_move; - std::string param_ns; - ROS_topics ros_topics; + COMMON_parameters common_par; /// internal node state variables /// internalState is; auxData aux_data; @@ -226,9 +227,9 @@ int main(int argc, char **argv) { std::vector playVector; file_data.recordVect = &recordVector; file_data.playVect = &playVector; + double nonzero_move; /// player variables /// - std_msgs::String startup_msg; std::string startup_msg_tmp; double dsCounter, dtCounter; @@ -241,19 +242,17 @@ int main(int argc, char **argv) { ros::NodeHandle nh; /// read parameters from server /// //load common parameters - nh.getParam("/roboglue_ros_recorder/parameter_ns", param_ns); - nh.getParam(param_ns+"startup_msg", startup_msg_tmp); - nh.getParam(param_ns+"loop_rate", loopRate); - nh.getParam(param_ns+"msg_buffer", msgBufferSize); - nh.getParam(param_ns+"min_nonzero_move", nonzero_move); - nh.getParam(param_ns+"INstate", ros_topics.stateSub); - nh.getParam(param_ns+"INcommands", ros_topics.commandSub); - nh.getParam(param_ns+"INcoordinates", ros_topics.coordSub); - nh.getParam(param_ns+"OUTstate", ros_topics.statePub); - nh.getParam(param_ns+"OUTcommands", ros_topics.commandPub); - nh.getParam(param_ns+"OUTcoordinates", ros_topics.coordPub); + nh.getParam("/roboglue_ros_recorder/parameter_ns", common_par.param_ns); + loadCommonParameters(&nh, &common_par); + // TODO: inserire anche questi parametri tra quelli comuni + nh.getParam(common_par.param_ns+"min_nonzero_move", nonzero_move); + nh.getParam(common_par.param_ns+"startup_msg", startup_msg_tmp); + // 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+"joint_jump_tresh", robot_data.joint_jump_tresh); // load spin rate - nh.getParam("/roboglue_ros_recorder/loop_rate", loopRate); + nh.getParam("/roboglue_ros_recorder/loop_rate", common_par.loopRate); //load meta template filename nh.getParam("/roboglue_ros_recorder/meta_dir", file_data.meta_dir_name); nh.getParam("/roboglue_ros_recorder/meta_template", file_data.meta_template_name); @@ -266,32 +265,29 @@ int main(int argc, char **argv) { // load coordinate send operation mode (time/distance) nh.getParam("/roboglue_ros_recorder/point_group_mode", robot_data.point_group_mode); nh.getParam("/roboglue_ros_recorder/planning_mode", robot_data.planning_mode); - // load robot model information - nh.getParam(param_ns+"robot_model_name", robot_data.robot_model_name); - nh.getParam(param_ns+"move_group_name", robot_data.move_group_name); - nh.getParam(param_ns+"joint_jump_tresh", robot_data.joint_jump_tresh); + /// set console log level /// ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Warn); /// set spinner rate /// - ros::Rate loop_rate(loopRate); + ros::Rate loop_rate(common_par.loopRate); ROS_INFO("Recorder Node Started"); - ros::Subscriber command_sub = nh.subscribe(ros_topics.commandSub, static_cast(msgBufferSize), + ros::Subscriber command_sub = nh.subscribe(common_par.ros_topics.commandSub, static_cast(common_par.msgBufferSize), boost::bind(commandCallback, _1, &is, &pos_data, &rviz_data, &file_data)); - ros::Subscriber coordinate_sub = nh.subscribe(ros_topics.coordSub, static_cast(msgBufferSize), + ros::Subscriber coordinate_sub = nh.subscribe(common_par.ros_topics.coordSub, static_cast(common_par.msgBufferSize), boost::bind(coordinateCallback, _1, &is, &pos_data)); - ros::Subscriber state_sub = nh.subscribe(ros_topics.stateSub, static_cast(msgBufferSize), + 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 /// publisherMap publishers; - ros::Publisher command_pub = nh.advertise(ros_topics.commandPub, - static_cast(msgBufferSize)); - ros::Publisher coord_pub = nh.advertise(ros_topics.coordPub, - static_cast(msgBufferSize)); - ros::Publisher state_pub = nh.advertise(ros_topics.statePub, - static_cast(msgBufferSize)); + ros::Publisher command_pub = nh.advertise(common_par.ros_topics.commandPub, + static_cast(common_par.msgBufferSize)); + ros::Publisher coord_pub = nh.advertise(common_par.ros_topics.coordPub, + static_cast(common_par.msgBufferSize)); + ros::Publisher state_pub = nh.advertise(common_par.ros_topics.statePub, + static_cast(common_par.msgBufferSize)); /// build a list of publisher to pass to mqtt callback /// publishers[command_pub.getTopic()] = &command_pub; diff --git a/src/roboglue_ros/src/roboglue_utils.cpp b/src/roboglue_ros/src/roboglue_utils.cpp index ebad69d..fbd5588 100644 --- a/src/roboglue_ros/src/roboglue_utils.cpp +++ b/src/roboglue_ros/src/roboglue_utils.cpp @@ -2,6 +2,19 @@ /// Utilities funtion used in roboglue ros nodes /// +bool loadCommonParameters(ros::NodeHandle* nh, COMMON_parameters *p) { + nh->getParam(p->param_ns+"INstate", p->ros_topics.statePub); + nh->getParam(p->param_ns+"INcommands", p->ros_topics.commandSub); + nh->getParam(p->param_ns+"INcoordinates", p->ros_topics.coordSub); + nh->getParam(p->param_ns+"INinterface", p->ros_topics.coordSub); + nh->getParam(p->param_ns+"OUTstate", p->ros_topics.statePub); + nh->getParam(p->param_ns+"OUTcommands", p->ros_topics.commandPub); + nh->getParam(p->param_ns+"OUTcoordinates", p->ros_topics.coordPub); + nh->getParam(p->param_ns+"OUTinterface", p->ros_topics.interfacePub); + nh->getParam(p->param_ns+"loop_rate", p->loopRate); + nh->getParam(p->param_ns+"msg_buffer", p->msgBufferSize); +} + geometry_msgs::TwistStamped jstr2Twist(nlohmann::json cval){ static uint32_t seqId = 0; geometry_msgs::TwistStamped tw;