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

corerzioni nel caricamento dei parametri

Browse Source
This commit is contained in:
Emanuele
2019-10-29 13:11:23 +01:00
parent 8ff1c7e84a
commit a9a82c5fe2
6 changed files with 41 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

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

2
src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch
View File

@@ -1,6 +1,6 @@
<launch>
<!-- Set logger format -->
<env name="ROSCONSOLE_FORMAT" value="[${severity}] [${node}]: ${message}" />
<env name="ROSCONSOLE_FORMAT" value="[${severity}] [${node}][${line}]: ${message}" />
<!-- <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${node}] [${function}]: ${message}" />-->
<!-- Start ROS official URobotics controller -->

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

@@ -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<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>(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>(common_par.ros_topics.stateSub, static_cast<uint32_t>(common_par.msgBufferSize),
boost::bind(stateCallback, _1, &is, &aux_data));
ros::Subscriber interface_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.interfaceSub, static_cast<uint32_t>(common_par.msgBufferSize),
boost::bind(interfaceCallback, _1, &is));
/// advertise publish topics ///
publisherMap publishers;
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,
@@ -221,23 +232,19 @@ int main(int argc, char **argv) {
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));
ros::Publisher interface_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.interfacePub,
static_cast<uint32_t>(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<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>(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>(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
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");

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

@@ -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<pointRecord> tempPlayVector;
std::vector<geometry_msgs::PoseStamped> 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<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>(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>(common_par.ros_topics.stateSub, static_cast<uint32_t>(common_par.msgBufferSize),
boost::bind(stateCallback, _1, &is, &aux_data));
ros::Subscriber interface_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.interfaceSub, static_cast<uint32_t>(common_par.msgBufferSize),
boost::bind(interfaceCallback, _1, &is));
/// advertise publish topics ///
publisherMap publishers;
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));
ros::Publisher interface_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.interfacePub,
static_cast<uint32_t>(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");

2
src/roboglue_ros/src/roboglue_relay.cpp
View File

@@ -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,"");

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

@@ -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){