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

Prima implementazione messaggio startup dei moduli

Browse Source
This commit is contained in:
Emanuele
2019-10-28 15:39:40 +01:00
parent 7e9dbe38e1
commit 5acd7d634d
4 changed files with 64 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
src/roboglue_ros/launch/roboglue_ros_urdriver.launch
View File

@@ -40,6 +40,7 @@
<!-- Message Relay to/from MQTT -->
<node name="roboglue_ros_relay" pkg="roboglue_ros" type="roboglue_ros_relay" output="screen" required="true">
<param name="parameter_ns" type="string" value="/roboglue_ros_common/" />
<param name="startup_msg" type="string" value='{"STARTUP":"RELAY"}'>
<param name="mqtt_host" type="string" value="tcp://localhost:1883" />
<param name="mqtt_qos" type="int" value="0" />
<param name="mqtt_INstate" type="string" value="roboglue_com/com2ros/state" />
@@ -53,12 +54,14 @@
<!-- Real Time Tracking -->
<node name="roboglue_ros_follower" pkg="roboglue_ros" type="roboglue_ros_follower" output="screen" required="true">
<param name="parameter_ns" type="string" value="roboglue_ros_common" />
<param name="startup_msg" type="string" value='{"STARTUP":"FOLLOWER"}'>
<param name="jog_pub_rate" type="int" value="5" />
</node>
<!-- Tracking data Recorder/Player -->
<node name="roboglue_ros_recorder" pkg="roboglue_ros" type="roboglue_ros_recorder" output="screen" required="true">
<param name="parameter_ns" type="string" value="/roboglue_ros_common/" />
<param name="startup_msg" type="string" value='{"STARTUP":"RECORDER"}'>
<param name="point_group_mode" type="string" value="dist" />
<param name="planning_mode" type="string" value="path" />
<param name="meta_dir" type="string" value="/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src/roboglue_ros/config/meta/" />
@@ -69,7 +72,4 @@
<param name="data_template" type="string" value="@data_template.data" />
</node>
<!-- <param name="planning_mode" type="string" value="joint" /> -->
<!-- <param name="point_group_mode" type="string" value="time" /> -->
</launch>

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

@@ -9,7 +9,7 @@
20190308 - Inserita una funzione che si occupa di effettuare il blocco delle variaili.
20190325 - Implementato uso del ros parameter server per la configurazione del nodo.
Contiene i nomi delle code ros.
20191023 - Passaggio dei commenti a GIT
20191023 - Spostata Documentazione su GIT
*/
////////// ROS INCLUDES /////////////
#include "ros/ros.h"
@@ -64,8 +64,8 @@ void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, p
} else {
ROS_WARN("Unknown command: %s", command.c_str());
}
} catch (nlohmann::json::exception){
ROS_ERROR("Failed to parse command!");
} catch (nlohmann::json::exception &e){
ROS_ERROR("Failed to parse COMMAND: [%s]", e.what());
}
}
@@ -178,6 +178,7 @@ int main(int argc, char **argv) {
/// node variables ///
int loopRate, msgBufferSize, jogPubRate;
std::string param_ns;
std_msgs::String startup_msg;
ROS_topics ros_topics;
/// internal state struct ////
internalState is;
@@ -191,6 +192,7 @@ int main(int argc, char **argv) {
ros::NodeHandle nh;
///read parameters from server ///
nh.getParam("/roboglue_ros_relay/parameter_ns", param_ns);
nh.getParam(param_ns+"startup_msg", startup_msg.data);
nh.getParam(param_ns+"loop_rate", loopRate);
nh.getParam(param_ns+"msg_buffer", msgBufferSize);
nh.getParam(param_ns+"INstate", ros_topics.stateSub);
@@ -210,7 +212,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::Debug);
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
ROS_INFO("Follower Node Started");
/// advertise publish topics ///
@@ -221,10 +223,22 @@ int main(int argc, char **argv) {
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));
/// 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 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>(ros_topics.commandSub, static_cast<uint32_t>(msgBufferSize),
boost::bind(commandCallback, _1, &is, &publishers, &pos_data, &robot_data));
@@ -247,13 +261,18 @@ int main(int argc, char **argv) {
boost::bind(timedPublishCallback, _1, &is, &publishers, &pos_data, &robot_data));
// now we can get inverse kinematics from kinematic state using the joint model
///////////////////// MAIN LOOP //////////////////////
bool startCycle = true;
while (ros::ok() && is.isRunning) {
if (startCycle){
startCycle = false;
publishers[command_pub.getTopic()]->publish(startup_msg);
}
ROS_DEBUG_ONCE("Follower Node Looping");
//testFunction(&trj_pub);
ros::spinOnce();
//if (is.isRealtime)
loop_rate.sleep();
}
//////////////////////////////////////////////////////
return 0;
}
////////////////////////////////////////

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

@@ -33,7 +33,7 @@
che non si incappi in configurazioni instabili.
20190708 - Implementazione funzioni di visualizzazione del percorso e pulizia dello schermo prima
dell'esecuzione del file.
20191029 - Spostata la documentazione si Git
20191029 - Spostata la documentazione suG GIT
*/
////////// ROS INCLUDES /////////////
#include "ros/ros.h"
@@ -125,7 +125,7 @@ void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, p
ROS_WARN("Unknown command: %s", command.c_str());
}
} catch (nlohmann::json::exception &e) {
ROS_ERROR("Failed to parse command: [%s]", e.what());
ROS_ERROR("Failed to parse COMMAND: [%s]", e.what());
is->isPlaying = false;
is->isRealtime = false;
is->isRecording = false;
@@ -181,22 +181,26 @@ void coordinateCallback(const std_msgs::String::ConstPtr& msg, internalState* is
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());
nlohmann::json auxParsed = nlohmann::json::parse(msg->data);
nlohmann::json digitalParsed = auxParsed["digital"];
nlohmann::json analogParsed = auxParsed["analog"];
std::vector<std::pair<std::string,bool>> tempDigital;
std::vector<std::pair<std::string,uint16_t>> tempAnalog;
if (is->isRecording || is->isRealtime){
for (auto di : digitalParsed.items()){
tempDigital.push_back(std::pair<std::string,bool>(di.key(),di.value().get<bool>()));
try {
nlohmann::json auxParsed = nlohmann::json::parse(msg->data);
nlohmann::json digitalParsed = auxParsed["digital"];
nlohmann::json analogParsed = auxParsed["analog"];
std::vector<std::pair<std::string,bool>> tempDigital;
std::vector<std::pair<std::string,uint16_t>> tempAnalog;
if (is->isRecording || is->isRealtime){
for (auto di : digitalParsed.items()){
tempDigital.push_back(std::pair<std::string,bool>(di.key(),di.value().get<bool>()));
}
for (auto ai : analogParsed.items()){
tempAnalog.push_back(std::pair<std::string,uint16_t>(ai.key(),ai.value().get<uint16_t>()));
}
aux->digital.clear();
aux->analog.clear();
aux->digital.assign(tempDigital.begin(), tempDigital.end());
aux->analog.assign(tempAnalog.begin(), tempAnalog.end());
}
for (auto ai : analogParsed.items()){
tempAnalog.push_back(std::pair<std::string,uint16_t>(ai.key(),ai.value().get<uint16_t>()));
}
aux->digital.clear();
aux->analog.clear();
aux->digital.assign(tempDigital.begin(), tempDigital.end());
aux->analog.assign(tempAnalog.begin(), tempAnalog.end());
} catch (nlohmann::json::exception &e) {
ROS_ERROR("Failed to parse STATE: [%s]", e.what());
}
}
@@ -224,6 +228,8 @@ int main(int argc, char **argv) {
file_data.playVect = &playVector;
/// player variables ///
std_msgs::String startup_msg;
double dsCounter, dtCounter;
std::vector<pointRecord> tempPlayVector;
std::vector<geometry_msgs::PoseStamped> tempPlanningVector;
@@ -235,6 +241,7 @@ int main(int argc, char **argv) {
/// read parameters from server ///
//load common parameters
nh.getParam("/roboglue_ros_recorder/parameter_ns", param_ns);
nh.getParam(param_ns+"startup_msg", startup_msg.data);
nh.getParam(param_ns+"loop_rate", loopRate);
nh.getParam(param_ns+"msg_buffer", msgBufferSize);
nh.getParam(param_ns+"min_nonzero_move", nonzero_move);
@@ -311,7 +318,9 @@ int main(int argc, char **argv) {
rviz_data.vtools->loadRemoteControl();
rviz_data.vtools->deleteAllMarkers();
rviz_data.vtools->trigger();
publishers[command_pub.getTopic()]->publish(startup_msg);
}
ROS_DEBUG_ONCE("Recorder Node Looping");
ros::spinOnce();
/////////////////////////////////////////
/////////// PLAY FILE ///////////////////

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

@@ -6,6 +6,7 @@
modi che li devono interpretare. qui viene trattato tutto come stringa.
20190325 - Implementato uso del ros parameter server per la configurazione del nodo.
Contiene i nomi delle code sia ros che mqtt.
20191028 - Documentazione Spostata su GIT
*/
////////// ROS INCLUDES /////////////
@@ -83,11 +84,13 @@ int main(int argc, char **argv) {
std::string param_ns;
MQTT_topics mqtt_topics;
ROS_topics ros_topics;
std_msgs::String startup_msg;
/// setup node parameters ///
ros::init(argc, argv, "roboglue_relay");
ros::NodeHandle nh;
///read parameters from server ///
nh.getParam("/roboglue_ros_relay/parameter_ns", param_ns);
nh.getParam(param_ns+"startup_msg", startup_msg.data);
nh.getParam(param_ns+"loop_rate", loopRate);
nh.getParam(param_ns+"msg_buffer", msgBufferSize);
nh.getParam(param_ns+"INstate", ros_topics.stateSub);
@@ -146,13 +149,18 @@ int main(int argc, char **argv) {
ROS_ERROR("Failed to Connect to MQTT: [%s]", e.what());
ros::shutdown();
}
////////////// MAIN LOOP /////////////
bool startCycle = true;
while (ros::ok()) {
if (startCycle){
startCycle = false;
publishers[command_pub.getTopic()]->publish(startup_msg);
}
ROS_DEBUG_ONCE("Relay Node Looping");
ros::spinOnce();
loop_rate.sleep();
}
///////////////////////////////////////
try {
client.stop_consuming();
client.disable_callbacks();