Skip to content

Commit

Permalink
use ros::spinOnce to publish on each simulation loops and debug (ref f…
Browse files Browse the repository at this point in the history 
…kanehiro#91)
  • Loading branch information
@yosuke
yosuke committed Sep 27, 2018
1 parent c93618c commit 1e8a41a
Show file tree
Hide file tree
Showing 2 changed files with 53 additions and 67 deletions.
101 changes: 46 additions & 55 deletions choreonoid_plugins/src/WorldRosItem.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,8 +18,6 @@

using namespace cnoid;

#define DEBUG_WORLDROSITEM 0

/*
Namepsace of parent of topics and services.
NOTE: If you renaming, do not include '-'.
Expand All @@ -43,15 +41,15 @@ void WorldRosItem::initialize(ExtensionManager* ext)
WorldRosItem::WorldRosItem()
{
publish_clk_update_rate_ = 100.0;
publish_ls_update_rate_ = 100.0;
publish_ms_update_rate_ = 100.0;
publish_cs_update_rate = 100.0;
publish_ls_update_rate_ = 10.0;
publish_ms_update_rate_ = 10.0;
publish_cs_update_rate = 0.0;
is_csmsg_verbose = false;

registration_node_management_.clear();
post_dynamics_function_regid.clear();
hooked_simulators_.clear();
post_dynamics_function_regid = -1;

RootItem::instance()->sigTreeChanged().connect(boost::bind(&WorldRosItem::registrationNodeStartAndStop, this));
RootItem::instance()->sigTreeChanged().connect(boost::bind(&WorldRosItem::hookSimulationStartAndStopEvent, this));
}

WorldRosItem::WorldRosItem(const WorldRosItem& org)
Expand All @@ -63,10 +61,10 @@ WorldRosItem::WorldRosItem(const WorldRosItem& org)
publish_cs_update_rate = org.publish_cs_update_rate;
is_csmsg_verbose = org.is_csmsg_verbose;

registration_node_management_.clear();
post_dynamics_function_regid.clear();
hooked_simulators_.clear();
post_dynamics_function_regid = -1;

RootItem::instance()->sigTreeChanged().connect(boost::bind(&WorldRosItem::registrationNodeStartAndStop, this));
RootItem::instance()->sigTreeChanged().connect(boost::bind(&WorldRosItem::hookSimulationStartAndStopEvent, this));
}

WorldRosItem::~WorldRosItem()
Expand Down Expand Up @@ -239,10 +237,10 @@ void WorldRosItem::publishContactsState()
return;
}

void WorldRosItem::registrationNodeStartAndStop()
void WorldRosItem::hookSimulationStartAndStopEvent()
{
WorldItemPtr parent;
std::map<SimulatorItemPtr, std::string>::iterator it;
std::map<std::string, bool>::iterator it;

if (! (parent = this->findOwnerItem<WorldItem>())) {
return;
Expand All @@ -252,18 +250,14 @@ void WorldRosItem::registrationNodeStartAndStop()
SimulatorItemPtr p = dynamic_cast<SimulatorItem*>(child);

if (p && ! p->isRunning()) {
it = registration_node_management_.find(p);

if (it == registration_node_management_.end()) {
it = hooked_simulators_.find(p->name());
if (it == hooked_simulators_.end()) {
p->sigSimulationStarted().connect(std::bind(&WorldRosItem::start, this));
p->sigSimulationFinished().connect(std::bind(&WorldRosItem::stop, this));

registration_node_management_[p] = p->name();
#if (DEBUG_WORLDROSITEM > 0)
std::cout << registration_node_management_[p] << ": regsitered" << std::endl;
hooked_simulators_[p->name()] = true;
ROS_DEBUG("Hooked simulator %s", p->name().c_str());
} else {
std::cout << it->second << ": already registered" << std::endl;
#endif /* (DEBUG_WORLDROSITEM > 0) */
ROS_DEBUG("Simulator %s is already hooked", it->first.c_str());
}
}
}
Expand All @@ -282,56 +276,53 @@ void WorldRosItem::start()
ROS_DEBUG("Found WorldItem: %s", world->name().c_str());
ROS_DEBUG("Found SimulatorItem: %s", sim->name().c_str());

double now = sim->currentTime();

rosnode_ = boost::shared_ptr<ros::NodeHandle>(new ros::NodeHandle(cnoidrospkg_parent_namespace_));

if (publish_clk_update_rate_ > 0.0 && publish_clk_update_rate_ <= 1000.0) {
// apply uppper limits
if (publish_clk_update_rate_ > 1000.0) publish_clk_update_rate_ = 1000.0;
if (publish_ls_update_rate_ > 1000.0) publish_ls_update_rate_ = 1000.0;
if (publish_ms_update_rate_ > 1000.0) publish_ms_update_rate_ = 1000.0;
if (publish_cs_update_rate > 1000.0) publish_cs_update_rate = 1000.0;

if (publish_clk_update_rate_ > 0.0) {
pub_clock_ = rosnode_->advertise<rosgraph_msgs::Clock>("/clock", 10);
publish_clk_update_interval_ = 1.0 / publish_clk_update_rate_;
publish_clk_next_time_ = 0.0;

post_dynamics_function_regid.push_back(
sim->addPostDynamicsFunction(std::bind(&WorldRosItem::publishClock, this)));
publish_clk_next_time_ = now + publish_clk_update_interval_;
}

if (publish_ls_update_rate_ > 0.0 && publish_ls_update_rate_ <= 1000.0) {
if (publish_ls_update_rate_ > 0.0) {
pub_link_states_ = rosnode_->advertise<gazebo_msgs::LinkStates>("link_states", 10);
publish_ls_update_interval_ = 1.0 / publish_ls_update_rate_;
publish_ls_next_time_ = 0.0;

post_dynamics_function_regid.push_back(
sim->addPostDynamicsFunction(std::bind(&WorldRosItem::publishLinkStates, this)));
publish_ls_next_time_ = now + publish_ls_update_interval_;
}

if (publish_ms_update_rate_ > 0.0 && publish_ms_update_rate_ <= 1000.0) {
if (publish_ms_update_rate_ > 0.0) {
pub_model_states_ = rosnode_->advertise<gazebo_msgs::ModelStates>("model_states", 10);
publish_ms_update_interval_ = 1.0 / publish_ms_update_rate_;
publish_ms_next_time_ = 0.0;

post_dynamics_function_regid.push_back(
sim->addPostDynamicsFunction(std::bind(&WorldRosItem::publishModelStates, this)));
publish_ms_next_time_ = now + publish_ms_update_interval_;
}

if (publish_cs_update_rate > 0.0 && publish_cs_update_rate <= 1000.0) {
if (publish_cs_update_rate > 0.0) {
std::string topic_name = world->name() + "/physics/contacts";
std::replace(topic_name.begin(), topic_name.end(), '-', '_');

pub_world_contacts_state_ = rosnode_->advertise<gazebo_msgs::ContactsState>(topic_name, 10);
sim_access_ = static_cast<WorldRosSimulatorItemAccessor*>(sim.get());
publish_cs_update_interval = 1.0 / publish_cs_update_rate;
publish_cs_next_time = 0.0;

post_dynamics_function_regid.push_back(
sim->addPostDynamicsFunction(std::bind(&WorldRosItem::publishContactsState, this)));
publish_cs_next_time = now + publish_cs_update_interval;

AISTSimulatorItem* aist_sim;

if ((aist_sim = dynamic_cast<AISTSimulatorItem*>(sim.get()))) {
aist_sim->setConstraintForceOutputEnabled(true);
}

contacts_state.states.clear();
}

post_dynamics_function_regid = sim->addPostDynamicsFunction(std::bind(&WorldRosItem::onPostDynamics, this));

std::string pause_physics_service_name("pause_physics");
ros::AdvertiseServiceOptions pause_physics_aso =
ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
Expand Down Expand Up @@ -388,12 +379,18 @@ void WorldRosItem::start()
ros::VoidPtr(), &rosqueue_);
delete_model_service_ = rosnode_->advertiseService(delete_aso);

async_ros_spin_.reset(new ros::AsyncSpinner(0));
async_ros_spin_->start();

rosqueue_thread_.reset(new boost::thread(&WorldRosItem::queueThread, this));
}

void WorldRosItem::onPostDynamics()
{
if (publish_clk_update_rate_ > 0.0) publishClock();
if (publish_ls_update_rate_ > 0.0) publishLinkStates();
if (publish_ms_update_rate_ > 0.0) publishModelStates();
if (publish_cs_update_rate > 0.0) publishContactsState();
ros::spinOnce();
}

void WorldRosItem::publishClock()
{
if (publish_clk_next_time_ > sim->currentTime()) {
Expand Down Expand Up @@ -594,20 +591,16 @@ bool WorldRosItem::deleteModel(gazebo_msgs::DeleteModel::Request &req,

void WorldRosItem::stop()
{
while (! post_dynamics_function_regid.empty()) {
sim->removePostDynamicsFunction(post_dynamics_function_regid.back());
post_dynamics_function_regid.pop_back();
if (post_dynamics_function_regid != -1) {
sim->removePostDynamicsFunction(post_dynamics_function_regid);
post_dynamics_function_regid = -1;
}

if (ros::ok()) {
if (rosqueue_.isEnabled()) {
rosqueue_.clear();
}

if (async_ros_spin_) {
async_ros_spin_->stop();
}

if (rosnode_) {
rosnode_->shutdown();
}
Expand All @@ -617,7 +610,5 @@ void WorldRosItem::stop()
rosqueue_thread_->join();
}

registration_node_management_.clear();

return;
}
19 changes: 7 additions & 12 deletions choreonoid_plugins/src/WorldRosItem.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -94,27 +94,22 @@ class CNOID_EXPORT WorldRosItem : public Item
WorldItemPtr world;
SimulatorItemPtr sim;
boost::shared_ptr<ros::NodeHandle> rosnode_;
boost::shared_ptr<ros::AsyncSpinner> async_ros_spin_;
ros::CallbackQueue rosqueue_;
boost::shared_ptr<boost::thread> rosqueue_thread_;

/**
The variable for managing registration to the simulator item.
This registration that function of node creation at the simulation start,
node deletion at the simulation stop.
*/
std::map<SimulatorItemPtr, std::string> registration_node_management_;

/// The registration id of calling function from physics engine. (physics engine is SimulatorItem's subclass)
std::list<int> post_dynamics_function_regid;
int post_dynamics_function_regid;

void queueThread();

std::map<std::string, bool> hooked_simulators_;

/**
@brief Connect to event signal, function of nodes creation and deletion.
The event signal is 'RootItem::instace()->sigTreeChanged()'.
@brief Hook simulation start and stop event
*/
void registrationNodeStartAndStop();
void hookSimulationStartAndStopEvent();

void onPostDynamics();

/*
For services and topics.
Expand Down

0 comments on commit 1e8a41a

Please sign in to comment.