Use reservation instead of resizing to initialize publisher vectors

[ssr-yuki]

Assigning elements after resizing a vector is generally redundant: resizing calls the constructor of ros::Publisher for N times by default, but all constructed objects are discarded by the assigns.

This PR proposes to use pairs of reserve and push_back instead. Improvement of process efficiency is expected.

[s-nakaoka]

The modified code seems to redundantly increase the number of publishers for each sensor type every time the simulation is started. This problem can be solved by clearing the vector elements of each sensor type in the createSensors function. The original code does not have this problem. In the first place, the cost of initializing the publisher's null handle is probably very small, and since the initialization is done only once at the beginning of the simulation, this part of the code has little impact on the efficiency of the simulation. In addition, although vector indices are used in the arguments of the callback function, their entities are added by push_back rather than by index specification, so the correspondence between the two is not clear in the code. Considering the above, I'm afraid there is no good reason why this pull request should be accepted.

[s-nakaoka]

It is true that local efficiency can be increased by using the reserve and push_back functions. To apply this, it would be better to use a code like the following, based on what I pointed out above.

force_sensor_publishers_.clear();
force_sensor_publishers_.reserve(forceSensors_.size());
for(auto& sensor : forceSensors){
    std::string name = sensor->name();
    std::replace(name.begin(), name.end(), '-', '_');
    auto publisher = rosnode_->advertise<geometry_msgs::WrenchStamped>(name, 1);
    sensor->sigStateChanged().connect(
        [this, sensor, publisher](){ updateForceSensor(sensor, publisher); });
    force_sensor_publishers_.push_back(publisher);
    ROS_INFO("Create force sensor %s", sensor->name().c_str());
}

[ssr-yuki]

I agree with your suggestion.

Furthermore, your suggestion would resolve a bug:

This problem can be solved by clearing the vector elements of each sensor type in the createSensors function.

We have to do this clearing first in calling createSensors() everytime, because the number of sensors can be changed by reloading the robot models between simulations, I guess.

E.g., a robot with 3 cameras is used in the 1st simulation, and the number of camera decreases by editing and reloading the robot model, then the vision publisher with index 3, undesirable one, still remains in the 2nd simulation.

[s-nakaoka]

Thank you for your contribution!