Open Ronan0912 opened 11 years ago
I doubt that just resetting the boolean flag to False will allow to reinitialize cleanly. There is more global state in rospy than that flag. If you want to give it a try you can reset the boolean flag to False manually and than call init again. But I am pretty sure that the first initialization will have more side effects which wont make the second init behave the same as the first.
I tried to reset the boolean flag to false. As you thought, the first initialization has more side effects because it always keeps the last master's IP address ... The enhancement is a little more important than what I expected. I'll look at another way.
Here is how I dynamically change a running ROS node between multiple ROS masters using only production APIs (i.e. no ROS code changes required using ROS Indigo):
Step 1. Launch ROS master 1 on localhost port 11311:
$ export ROS_MASTER_URI=http://localhost:11311 $ roscore
Step 2. Launch ROS master 2 on localhost port 11312:
$ export ROS_MASTER_URI=http://localhost:11312 $ roscore -p 11312
Step 3. Launch the sample publisher (see below) with the name "publisher_11311" that publishes to ROS master 1 on localhost port 11311:
$ publisher __name:=publisher_11311 __master:=http://localhost:11311
Step 4. Launch the same sample publisher with the name "publisher_11312" that publishes to ROS master 2 on localhost port 11312:
$ publisher __name:=publisher_11312 __master:=http://localhost:11312
Step 5. Run the sample subscriber (see below) that listens to ROS master 1 for 10 messages and then dynamically switches to ROS master 2 and listens to it for 10 messages:
Sample publisher:
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <iostream>
int main(int argc, char **argv)
{
ros::init(argc, argv, "publisher");
ros::NodeHandle node_handle;
ros::Publisher publisher = node_handle.advertise<std_msgs::String>("hello_publisher", 1000);
ros::Rate loop_rate(10);
int count = 0;
while (ros::ok())
{
std_msgs::String msg;
std::stringstream ss;
ss
<< "Hello from "
<< ros::this_node::getName()
<< ", count = "
<< ++count;
msg.data = ss.str();
//std::cout << msg.data << "\n";
publisher.publish(msg);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
Sample subscriber:
#include "ros/ros.h"
#include "ros/callback_queue.h"
#include "std_msgs/String.h"
#include <iostream>
namespace ros {
namespace master {
void init(const M_string& remappings);
}
}
void callback(const std_msgs::String::ConstPtr& msg) {
std::cout << "I heard: [" << msg->data << "]\n";
}
int main(int argc, char **argv)
{
const std::string topic_name = "hello_publisher";
const std::string node_name = "subscriber";
int count = 0;
ros::M_string remappings;
ros::init(argc, argv, node_name);
remappings["__master"] = "http://localhost:11311";
ros::master::init(remappings);
{
ros::NodeHandle node_handle;
ros::Subscriber subscriber = node_handle.subscribe(topic_name, 1000, callback);
count = 10;
do {
ros::getGlobalCallbackQueue() -> callAvailable(ros::WallDuration(1));
} while (ros::ok() && --count > 0);
}
remappings["__master"] = "http://localhost:11312";
ros::master::init(remappings);
{
ros::NodeHandle node_handle;
ros::Subscriber subscriber = node_handle.subscribe(topic_name, 1000, callback);
count = 10;
do {
ros::getGlobalCallbackQueue() -> callAvailable(ros::WallDuration(1));
} while (ros::ok() && --count > 0);
}
return 0;
}
Currently, in the ros::init function, a global boolean (g_initialized) is set to true after the initializations. If you call a second time the ros::init function (after a ros::shutdown), nothing will happen because there is a verification of the boolean value. In our application, a ROS node on the iOS platform (https://github.com/introlab/ros_for_ios), we need to be able to change the ROS master's IP on a configuration screen (it exports the ROS_MASTER_URI and after calls ros::init). Indeed, an iOS application is always active and can't be reset easily. Could it be possible to add a functionality of reinitialization (for example a function which sets the g_initialized boolean to false) without side effects ?
Thanks in advance.