From a9a82c5fe203343a8274aa05223b15b888fe851c Mon Sep 17 00:00:00 2001 From: Emanuele Date: Tue, 29 Oct 2019 13:11:23 +0100 Subject: [PATCH] corerzioni nel caricamento dei parametri --- .../include/roboglue_ros/roboglue_utils.h | 1 + .../roboglue_ros_urdriver_norviz.launch | 2 +- src/roboglue_ros/src/roboglue_follower.cpp | 37 +++++++++++-------- src/roboglue_ros/src/roboglue_recorder.cpp | 18 +++++---- src/roboglue_ros/src/roboglue_relay.cpp | 2 +- src/roboglue_ros/src/roboglue_utils.cpp | 8 +++- 6 files changed, 41 insertions(+), 27 deletions(-) diff --git a/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h b/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h index 8f52fd2..1726e82 100644 --- a/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h +++ b/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h @@ -52,6 +52,7 @@ typedef struct { ROS_topics ros_topics; int loopRate, msgBufferSize; std::string param_ns; + std_msgs::String startupMsg; } COMMON_parameters; ///////// AUX DATA STORAGE /////////////// diff --git a/src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch b/src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch index 844e17c..04e8000 100644 --- a/src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch +++ b/src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch @@ -1,6 +1,6 @@ - + diff --git a/src/roboglue_ros/src/roboglue_follower.cpp b/src/roboglue_ros/src/roboglue_follower.cpp index 5090472..1fdabfc 100644 --- a/src/roboglue_ros/src/roboglue_follower.cpp +++ b/src/roboglue_ros/src/roboglue_follower.cpp @@ -113,6 +113,10 @@ void stateCallback(const std_msgs::String::ConstPtr& msg, internalState* is, 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 timedPublishCallback(const ros::TimerEvent&, internalState* is, publisherMap* pubs, positionData* pos, robotData* rdata){ static geometry_msgs::TwistStamped lasttwPose; static trajectory_msgs::JointTrajectory lastjtPose; @@ -177,8 +181,6 @@ void timedPublishCallback(const ros::TimerEvent&, internalState* is, publisherMa int main(int argc, char **argv) { /// node variables /// COMMON_parameters common_par; - std::string startup_msg_tmp; - std_msgs::String startup_msg; /// internal state struct //// internalState is; positionData pos_data; @@ -190,25 +192,34 @@ int main(int argc, char **argv) { /// setup node parameters /// ros::init(argc, argv, "roboglue_follower"); ros::NodeHandle nh; + publisherMap publishers; ///read parameters from server /// 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); + // 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_follower/jog_pub_rate", jogPubRate); + /// 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"); + /// 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 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(common_par.ros_topics.stateSub, static_cast(common_par.msgBufferSize), + boost::bind(stateCallback, _1, &is, &aux_data)); + ros::Subscriber interface_sub = nh.subscribe(common_par.ros_topics.interfaceSub, static_cast(common_par.msgBufferSize), + boost::bind(interfaceCallback, _1, &is)); /// advertise publish topics /// - publisherMap publishers; 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, @@ -221,23 +232,19 @@ int main(int argc, char **argv) { static_cast(common_par.msgBufferSize)); ros::Publisher state_pub = nh.advertise(common_par.ros_topics.statePub, static_cast(common_par.msgBufferSize)); + ros::Publisher interface_pub = nh.advertise(common_par.ros_topics.interfacePub, + static_cast(common_par.msgBufferSize)); /// build a list of publisher to pass to mqtt callback /// publishers[command_pub.getTopic()] = &command_pub; publishers[coord_pub.getTopic()] = &coord_pub; publishers[state_pub.getTopic()] = &state_pub; + publishers[interface_pub.getTopic()] = &interface_pub; /// publishers to jog command interfaces publishers["jog_pub"] = &jog_pub; publishers["jog_joint_pub"] = &jog_joint_pub; publishers["trj_pub"] = &trj_pub; - /// 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 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(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 moveit::planning_interface::MoveGroupInterface move_group(robot_data.move_group_name); @@ -258,8 +265,8 @@ int main(int argc, char **argv) { while (ros::ok() && is.isRunning) { if (startCycle){ startCycle = false; - startup_msg.data = startup_msg_tmp.c_str(); - publishers[command_pub.getTopic()]->publish(startup_msg); + common_par.startupMsg.data = common_par.startupMsg.data + ros::this_node::getName(); + publishers[command_pub.getTopic()]->publish(common_par.startupMsg); ros::spinOnce(); } ROS_DEBUG_ONCE("Follower Node Looping"); diff --git a/src/roboglue_ros/src/roboglue_recorder.cpp b/src/roboglue_ros/src/roboglue_recorder.cpp index bc12a72..02f3bfe 100644 --- a/src/roboglue_ros/src/roboglue_recorder.cpp +++ b/src/roboglue_ros/src/roboglue_recorder.cpp @@ -230,8 +230,6 @@ int main(int argc, char **argv) { double nonzero_move; /// player variables /// - std_msgs::String startup_msg; - std::string startup_msg_tmp; double dsCounter, dtCounter; std::vector tempPlayVector; std::vector tempPlanningVector; @@ -240,14 +238,14 @@ int main(int argc, char **argv) { /// setup node parameters /// ros::init(argc, argv, "roboglue_recorder"); ros::NodeHandle nh; + publisherMap publishers; /// read parameters from server /// //load common parameters 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+"min_nonzero_move", nonzero_move); 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); @@ -271,28 +269,32 @@ int main(int argc, char **argv) { ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Warn); /// set spinner rate /// ros::Rate loop_rate(common_par.loopRate); - ROS_INFO("Recorder Node Started"); + /// 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, &pos_data, &rviz_data, &file_data)); 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(common_par.ros_topics.stateSub, static_cast(common_par.msgBufferSize), boost::bind(stateCallback, _1, &is, &aux_data)); + ros::Subscriber interface_sub = nh.subscribe(common_par.ros_topics.interfaceSub, static_cast(common_par.msgBufferSize), + boost::bind(interfaceCallback, _1, &is)); /// advertise publish topics /// - publisherMap publishers; 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)); + ros::Publisher interface_pub = nh.advertise(common_par.ros_topics.interfacePub, + static_cast(common_par.msgBufferSize)); /// build a list of publisher to pass to mqtt callback /// publishers[command_pub.getTopic()] = &command_pub; publishers[coord_pub.getTopic()] = &coord_pub; publishers[state_pub.getTopic()] = &state_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 @@ -315,8 +317,8 @@ int main(int argc, char **argv) { rviz_data.vtools->loadRemoteControl(); rviz_data.vtools->deleteAllMarkers(); rviz_data.vtools->trigger(); - startup_msg.data = startup_msg_tmp.c_str(); - publishers[command_pub.getTopic()]->publish(startup_msg); + common_par.startupMsg.data = common_par.startupMsg.data + ros::this_node::getName(); + publishers[command_pub.getTopic()]->publish(common_par.startupMsg); ros::spinOnce(); } ROS_DEBUG_ONCE("Recorder Node Looping"); diff --git a/src/roboglue_ros/src/roboglue_relay.cpp b/src/roboglue_ros/src/roboglue_relay.cpp index 847a5b1..d9853f9 100644 --- a/src/roboglue_ros/src/roboglue_relay.cpp +++ b/src/roboglue_ros/src/roboglue_relay.cpp @@ -123,7 +123,7 @@ int main(int argc, char **argv) { /// set spinner rate /// ros::Rate loop_rate(loopRate); /// set console log level /// - ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Warn); + ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug); ROS_INFO("Relay Node Started"); /// initialize MQTT client /// mqtt::async_client client(mqtt_topics.mqttHost,""); diff --git a/src/roboglue_ros/src/roboglue_utils.cpp b/src/roboglue_ros/src/roboglue_utils.cpp index fbd5588..b406195 100644 --- a/src/roboglue_ros/src/roboglue_utils.cpp +++ b/src/roboglue_ros/src/roboglue_utils.cpp @@ -3,16 +3,20 @@ /// 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+"INstate", p->ros_topics.stateSub); 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+"INinterface", p->ros_topics.interfaceSub); 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+"OUTdelta_jog", p->ros_topics.delta_jog); + nh->getParam(p->param_ns+"OUTdelta_joint_jog", p->ros_topics.delta_joint_jog); + nh->getParam(p->param_ns+"OUTtraj_jog", p->ros_topics.traj_jog); nh->getParam(p->param_ns+"loop_rate", p->loopRate); nh->getParam(p->param_ns+"msg_buffer", p->msgBufferSize); + nh->getParam(p->param_ns+"startup_msg", p->startupMsg.data); } geometry_msgs::TwistStamped jstr2Twist(nlohmann::json cval){