Obbart/RoboGlue_ROS
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Weaponizzata la logica che carica i parametri da ROSparam

Browse Source 
Raggruppati tutti i parametri comuni in una struttura uguale per tutti i
nodi.
Aggiungere altri parametri alla lista dei comuni
This commit is contained in:
Emanuele
2019-10-29 11:47:18 +01:00
parent ad07d2f1ca
commit 8ff1c7e84a
4 changed files with 76 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
src/roboglue_ros/include/roboglue_ros/roboglue_utils.h
View File

@@ -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<std::pair<std::string,bool>> 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);

61
src/roboglue_ros/src/roboglue_follower.cpp
View File

@@ -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<std::string> 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<geometry_msgs::TwistStamped>(ros_topics.delta_jog,
static_cast<uint32_t>(msgBufferSize));
ros::Publisher jog_joint_pub = nh.advertise<geometry_msgs::TwistStamped>(ros_topics.delta_joint_jog,
static_cast<uint32_t>(msgBufferSize));
ros::Publisher trj_pub = nh.advertise<trajectory_msgs::JointTrajectory>(ros_topics.traj_jog,
static_cast<uint32_t>(msgBufferSize));
ros::Publisher command_pub = nh.advertise<std_msgs::String>(ros_topics.commandPub,
static_cast<uint32_t>(msgBufferSize));
ros::Publisher coord_pub = nh.advertise<std_msgs::String>(ros_topics.coordPub,
static_cast<uint32_t>(msgBufferSize));
ros::Publisher state_pub = nh.advertise<std_msgs::String>(ros_topics.statePub,
static_cast<uint32_t>(msgBufferSize));
ros::Publisher jog_pub = nh.advertise<geometry_msgs::TwistStamped>(common_par.ros_topics.delta_jog,
static_cast<uint32_t>(common_par.msgBufferSize));
ros::Publisher jog_joint_pub = nh.advertise<geometry_msgs::TwistStamped>(common_par.ros_topics.delta_joint_jog,
static_cast<uint32_t>(common_par.msgBufferSize));
ros::Publisher trj_pub = nh.advertise<trajectory_msgs::JointTrajectory>(common_par.ros_topics.traj_jog,
static_cast<uint32_t>(common_par.msgBufferSize));
ros::Publisher command_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.commandPub,
static_cast<uint32_t>(common_par.msgBufferSize));
ros::Publisher coord_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.coordPub,
static_cast<uint32_t>(common_par.msgBufferSize));
ros::Publisher state_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.statePub,
static_cast<uint32_t>(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<std_msgs::String>(ros_topics.commandSub, static_cast<uint32_t>(msgBufferSize),
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));
ros::Subscriber coordinate_sub = nh.subscribe<std_msgs::String>(ros_topics.coordSub, static_cast<uint32_t>(msgBufferSize),
ros::Subscriber coordinate_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.coordSub, static_cast<uint32_t>(common_par.msgBufferSize),
boost::bind(coordinateCallback, _1, &publishers, &pos_data));
ros::Subscriber state_sub = nh.subscribe<std_msgs::String>(ros_topics.stateSub, static_cast<uint32_t>(msgBufferSize),
ros::Subscriber state_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.stateSub, static_cast<uint32_t>(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

58
src/roboglue_ros/src/roboglue_recorder.cpp
View File

@@ -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<pointRecord> 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<std_msgs::String>(ros_topics.commandSub, static_cast<uint32_t>(msgBufferSize),
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, &pos_data, &rviz_data, &file_data));
ros::Subscriber coordinate_sub = nh.subscribe<std_msgs::String>(ros_topics.coordSub, static_cast<uint32_t>(msgBufferSize),
ros::Subscriber coordinate_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.coordSub, static_cast<uint32_t>(common_par.msgBufferSize),
boost::bind(coordinateCallback, _1, &is, &pos_data));
ros::Subscriber state_sub = nh.subscribe<std_msgs::String>(ros_topics.stateSub, static_cast<uint32_t>(msgBufferSize),
ros::Subscriber state_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.stateSub, static_cast<uint32_t>(common_par.msgBufferSize),
boost::bind(stateCallback, _1, &is, &aux_data));
/// advertise publish topics ///
publisherMap publishers;
ros::Publisher command_pub = nh.advertise<std_msgs::String>(ros_topics.commandPub,
static_cast<uint32_t>(msgBufferSize));
ros::Publisher coord_pub = nh.advertise<std_msgs::String>(ros_topics.coordPub,
static_cast<uint32_t>(msgBufferSize));
ros::Publisher state_pub = nh.advertise<std_msgs::String>(ros_topics.statePub,
static_cast<uint32_t>(msgBufferSize));
ros::Publisher command_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.commandPub,
static_cast<uint32_t>(common_par.msgBufferSize));
ros::Publisher coord_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.coordPub,
static_cast<uint32_t>(common_par.msgBufferSize));
ros::Publisher state_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.statePub,
static_cast<uint32_t>(common_par.msgBufferSize));
/// build a list of publisher to pass to mqtt callback ///
publishers[command_pub.getTopic()] = &command_pub;

13
src/roboglue_ros/src/roboglue_utils.cpp
View File

@@ -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;