-
Notifications
You must be signed in to change notification settings - Fork 1
Example explanation (CPP)
A simple example has been created to better explain CAFER's functionality using C++. This example shows how to define a simple CAFER client that does nothing but send a message containing a counter that is incremented, together to other simple available use examples.
First, the basic services of CAFER are launched.
roslaunch cafer_core cafer.launch
Then, a CAFER node and its corespondent component are created using the basic_example_launch_set_nodes.cpp file, executed by a launch file basic_example_launch_set_nodes.launch. Let's take a look to the launcher code:
<launch>
<arg name="ns" default="basic_ns"/>
<arg name="name" default="basic_example_new_node"/>
<arg name="management_topic" default="basic_mgmt" />
<arg name="frequency" default="30"/>
<arg name="creator_ns" default="unset"/>
<arg name="creator_id" default="-1"/>
<node name="$(arg name)" pkg="cafer_core" type="basic_example_new_node" ns="$(arg ns)" output="screen" >
<param name="frequency" type="double" value="$(arg frequency)" />
<param name="management_topic" type="string" value="$(arg management_topic)" />
<param name="creator_ns" type="string" value="$(arg creator_ns)"/>
<param name="creator_id" type="int" value="$(arg creator_id)"/>
<param name="created_ns" type="string" value="$(arg ns)"/>
</node>
</launch>This launch file defines a set of parameters, and executes the Python file where the functionality of the node is contained. The parameters defined are the namespace to add the node, a management topic to share information between the different nodes of the exercise, the message exchange frecuency, and the type of nodes to be used. These parameters are global for all the nodes of the example, and any of them can get them. It's revelevant to mention the output="screen" option of the node. If this option is provided the node will be able to print in the stardard output (usually a terminal). The launch file is executed as:
roslaunch cafer_core basic_example_launch_set_nodes.launch
Using the commands rosnode list & rostopic list we can check that a CAFER node named /basic_ns/basic_example_new_node is generated. Also, a management topic called /basic_ns/basic_example_new_node/basic_mgmt is created, together to the topic /basic_ns/basic_example_new_node/basic_example_topic, where the count is exposed.
Now, let's take a look to the functionality of the node basic_example_launch_set_nodes.cpp. First a dummy client is defined inheritating from the cafer_core::Component class, overriding its virtual methods:
class DummyClient : public cafer_core::Component {
using cafer_core::Component::Component; // To inherit Component's constructor
cafer_core::shared_ptr<ros::Publisher> dummy_p;
long int n;
public:
~DummyClient()
{ shutdown(); }
void client_connect_to_ros(void)
{
ROS_INFO_STREAM("DummyClient type=" << get_type() << " id=" << get_id() << " connecting to ROS");
dummy_p.reset(new ros::Publisher(my_ros_nh->advertise<std_msgs::Int64>("basic_example_topic", 10)));
_is_init = true;
}
void client_disconnect_from_ros(void)
{
ROS_INFO_STREAM("DummyClient type=" << get_type() << " id=" << get_id() << " disconnecting from ROS");
dummy_p.reset();
}
void update(void)
{ }
void init()
{ connect_to_ros(); }
void publish(int n)
{
if (is_connected_to_ros()) {
std_msgs::Int64 mi;
mi.data = n;
dummy_p->publish(mi);
}
}
};The init method is called during the execution of the Component constructor. This method calls to the Component's method connect_to_ros method, which uses the overriden method client_connect_to_ros of the dummy client to create a topic to publish the counter on it. The overriden publish method is in charge of the publication. The destructor of the dummy client calls to the Component method shutdown, which deletes the previously generated topic using the overriden method client_disconnect_from_ros.
A main method executes the functionality of the CAFER node:
int main(int argc, char **argv)
{
cafer_core::init(argc, argv, "basic_example_new_node");
std::string management_topic = "unset", type = "basic_example_new_node";
double freq;
cafer_core::ros_nh->getParam("management_topic", management_topic);
cafer_core::ros_nh->getParam("frequency", freq);
ROS_INFO_STREAM("Management topic for new node: " << management_topic << " ; Namespace: " <<
cafer_core::ros_nh->getNamespace());
DummyClient cc(management_topic, type, freq);
cc.wait_for_init();
int cpt = 0;
while (ros::ok() && (!cc.get_terminate())) {
cc.spin();
cc.publish(cpt);
cc.update();
cc.sleep();
cpt++;
}
cc.shutdown();
return 0;
}The parameters defined in the launch file are read, the dummy client is created, and the counter is published in a loop until the node receives a signal from the management topic to be killed, handled by the get_terminate Component method. The ros::ok method checks if the ROS' core is running. Within the loop, spin is called to check the management topic, a value is published (initially 0), sleep is called to respect the frecuency defined for the counter publisher, and the previous value is increased.
At this point the CAFER node is created, linked to the management topic. Some actions can be executed on this environment. For example, changing the frecuency of the message exchanges, basic_example_change_frequency:
rosrun cafer_core basic_example_change_frequency /basic_mgmt 40
Let's take a look to its code:
int main(int argc, char **argv)
{
int freq_value = 10;
if (argc != 3) {
ROS_INFO_STREAM(
"Correct use : rosrun cafer_core basic_example_change_frequency management_topic freq_value");
return 0;
}
else {
std::stringstream ss(argv[2]);
ss >> freq_value;
}
ros::init(argc, argv, "change_freq_node");
ros::NodeHandle nh;
std::string management_topic = argv[1];
ROS_INFO_STREAM("management_topic" << management_topic);
ros::Publisher p = nh.advertise<cafer_core::Management>(management_topic, freq_value);
sleep(1);
cafer_core::Management msg;
msg.type = cafer_core::CHG_FREQ;
msg.src_node = "";
msg.src_id = -1;
msg.dest_node = "all";
msg.dest_id = -1;
msg.data_int = 0;
msg.data_flt = freq_value;
msg.data_str = "";
ROS_INFO_STREAM("Publishing the message to change frequency");
p.publish(msg);
return 0;
}This node receives as input the management topic name to send the change message, and the desired frequency value. It creates the correspondant message and publish it to the topic. The already existing nodes will receive this message and will update their frecuency rate value.
It is also possible to kill all the nodes linked to a management topic, basic_example_kill_node_topic_type.
rosrun cafer_core basic_example_kill_node_topic_type /basic_mgmt basic_example_new_node
In its code, an empty dummy client is created to inherit the functionalities of the Component class to the CAFER node:
class DummyClient : public cafer_core::Component {
using cafer_core::Component::Component; // To inherit Component's constructor
long int n;
public:
void client_disconnect_from_ros(void)
{ }
void client_connect_to_ros(void)
{ }
void init(void)
{ }
void update(void)
{ }
};Then the node functionalities are executed:
int main(int argc, char **argv)
{
/** Parameters */
std::string management_topic, type;
if (argc != 3) {
ROS_INFO_STREAM("Correct use : rosrun cafer_core basic_example_kill_node_topic_type topic type ");
return 0;
}
else {
management_topic = argv[1];
type = argv[2];
}
/** New node */
cafer_core::init(argc, argv, "basic_example_kill_node_topic_type");
// cafer_core::ros_nh->getParam("/basic_example_ns/basic_example_new_node/management_topic",management_topic);
// cafer_core::ros_nh->getParam("/basic_example_ns/basic_example_new_node/type",type);
ROS_INFO_STREAM("Killing nodes from topic " << management_topic << " and type " << type);
/** Create component, in charge of calling the nodes to kill themself*/
DummyClient cc(management_topic, type);
cc.wait_for_init();
/** Check the number of current nodes */
int nb_nodes_to_kill = cc.how_many_client_from_type(type);
std::vector<cafer_core::ClientDescriptor> ntk;
if (nb_nodes_to_kill > 0) {
ROS_INFO_STREAM(nb_nodes_to_kill << " nodes of type " << type << " to be killed");
cc.get_connected_client_with_type(type, ntk);
}
/** Kill the nodes */
for (int i = 0; i < nb_nodes_to_kill; i++) {
cc.send_local_node_death(ntk[i].ns, ntk[i].id);
int cpt = 10;
while (cc.is_client_up(ntk[i].ns, ntk[i].id) && (cpt > 0)) {
ROS_INFO_STREAM("The node " << ntk[i].id << " is still up.");
cc.spin();
cc.sleep();
sleep(1);
cpt--;
}
ROS_INFO_STREAM("The node " << ntk[i].id << " is down.");
}
/** Check the number of nodes killed */
ROS_INFO_STREAM(cc.how_many_client_from_type(type) << " nodes available of type " << type);
return 0;
}It checks the number of nodes available with the provided type, and the component sends them an order to be killed. Finally, It's checked if any node with the same type is still alive