Skip to content

Commit

Permalink
optimizing and getting rid of busy loop
Browse files Browse the repository at this point in the history
  • Loading branch information
@jbohren
jbohren committed Oct 29, 2015
1 parent b6ce1ec commit 4830f30
Show file tree
Hide file tree
Showing 2 changed files with 128 additions and 121 deletions.
245 changes: 126 additions & 119 deletions assembly_sim/src/assembly_soup_plugin.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
#include <stdio.h>

#include <boost/make_shared.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/format.hpp>

Expand Down Expand Up @@ -453,102 +454,6 @@ namespace assembly_sim
boost::format("%s/%s")
% atom_name
% female_atom->model->type);

// Broadcast TF frames for this link
if(broadcast_tf_)
{

//gzwarn<<"broadcasting tf/marker info"<<std::endl;

tf::Transform tf_frame;
visualization_msgs::MarkerArray male_mate_markers;
visualization_msgs::MarkerArray female_mate_markers;

// Broadcast a tf frame for this link
to_tf(female_atom->link->GetWorldPose(), tf_frame);
br.sendTransform(
tf::StampedTransform(
tf_frame,
ros::Time::now(),
tf_world_frame_,
link_name));

// Broadcast all male mate points for this atom
for(std::vector<MatePointPtr>::iterator it_mmp = female_atom->male_mate_points.begin();
it_mmp != female_atom->male_mate_points.end();
++it_mmp)
{
MatePointPtr male_mate_point = *it_mmp;

const std::string male_mate_point_name = str(
boost::format("%s/male_%d")
% atom_name
% male_mate_point->model->id);

tf::poseKDLToTF(male_mate_point->model->pose,tf_frame);
br.sendTransform(
tf::StampedTransform(
tf_frame,
ros::Time::now(),
link_name,
male_mate_point_name));

visualization_msgs::Marker mate_marker;
mate_marker.header.frame_id = male_mate_point_name;
mate_marker.header.stamp = ros::Time(0);
mate_marker.type = mate_marker.CUBE;
mate_marker.action = mate_marker.ADD;
mate_marker.id = (iter * 100) + male_mate_point->model->id;
mate_marker.scale.x = 0.02;
mate_marker.scale.y = 0.02;
mate_marker.scale.z = 0.01;
mate_marker.color.r = 1.0;
mate_marker.color.g = 0.0;
mate_marker.color.b = 0.0;
mate_marker.color.a = 0.25;
male_mate_markers.markers.push_back(mate_marker);
}

// Broadcast all female mate points for this atom
for(std::vector<MatePointPtr>::iterator it_fmp = female_atom->female_mate_points.begin();
it_fmp != female_atom->female_mate_points.end();
++it_fmp)
{
MatePointPtr female_mate_point = *it_fmp;

const std::string female_mate_point_name = str(
boost::format("%s/female_%d")
% atom_name
% female_mate_point->model->id);

tf::poseKDLToTF(female_mate_point->model->pose, tf_frame);
br.sendTransform(
tf::StampedTransform(
tf_frame,
ros::Time::now(),
link_name,
female_mate_point_name));

visualization_msgs::Marker mate_marker;
mate_marker.header.frame_id = female_mate_point_name;
mate_marker.header.stamp = ros::Time(0);
mate_marker.type = mate_marker.CUBE;
mate_marker.action = mate_marker.ADD;
mate_marker.id = (iter * 100) + female_mate_point->model->id;
mate_marker.scale.x = 0.02;
mate_marker.scale.y = 0.02;
mate_marker.scale.z = 0.01;
mate_marker.color.r = 0.0;
mate_marker.color.g = 0.0;
mate_marker.color.b = 1.0;
mate_marker.color.a = 0.25;
female_mate_markers.markers.push_back(mate_marker);
}

male_mate_pub_.publish(male_mate_markers);
female_mate_pub_.publish(female_mate_markers);
}

// Iterate over all female mate points of female link
for(std::vector<MatePointPtr>::iterator it_fmp = female_atom->female_mate_points.begin();
it_fmp != female_atom->female_mate_points.end();
Expand Down Expand Up @@ -622,6 +527,9 @@ namespace assembly_sim
mate = mtf->second.at(male_mate_point);
}

// Skip this pair if it's already scheduled for attaching/detaching
if(mates_to_attach.count(mate) > 0 or mates_to_detach.count(mate)) { continue; }

// Compute the world pose of the male mate frame
// This takes into account the attachment displacement (anchor_offset)
KDL::Frame male_mate_frame = male_atom_frame * male_mate_point->model->pose * mate->anchor_offset;
Expand All @@ -633,21 +541,24 @@ namespace assembly_sim
if(mate->joint) {
//if(female_mate_point->model->id == 0) gzwarn<<" --- err linear: "<<twist_err.vel.Norm()<<" angular: "<<twist_err.rot.Norm()<<std::endl;

// Synchronize with main update thread
boost::mutex::scoped_lock update_lock(update_mutex_);

// Determine if mated atoms need to be detached
if(mate->joint->GetParent() and mate->joint->GetChild()) {
//gzwarn<<"Parts are mated!"<<std::endl;

if(twist_err.vel.Norm() > mate_model->detach_threshold_linear or
twist_err.rot.Norm() > mate_model->detach_threshold_angular)
{
mates_to_detach.push_back(mate);
mates_to_detach.insert(mate);
} else if(
twist_err.vel.Norm() < mate->mate_error.vel.Norm() and
twist_err.rot.Norm() < mate->mate_error.rot.Norm())
{
// re-mate but closer to the desired point
mate->mate_error = twist_err;
mates_to_attach.push_back(mate);
mates_to_attach.insert(mate);
}

if(publish_active_mates_) {
Expand All @@ -661,11 +572,12 @@ namespace assembly_sim
twist_err.rot.Norm() < mate_model->attach_threshold_angular)
{
mate->mate_error = twist_err;
mates_to_attach.push_back(mate);
mates_to_attach.insert(mate);
}
}

// Broadcast the TF frame for this joint
// TODO: move this introspection out of this thread
if (broadcast_tf_ and mate->joint->GetParent() and mate->joint->GetChild())
{
tf::Transform tf_joint_frame;
Expand Down Expand Up @@ -693,8 +605,105 @@ namespace assembly_sim
}
}
}

// Broadcast TF frames for this link
// TODO: move this introspection out of this thread
if(broadcast_tf_)
{

//gzwarn<<"broadcasting tf/marker info"<<std::endl;

tf::Transform tf_frame;
visualization_msgs::MarkerArray male_mate_markers;
visualization_msgs::MarkerArray female_mate_markers;

// Broadcast a tf frame for this link
to_tf(female_atom->link->GetWorldPose(), tf_frame);
br.sendTransform(
tf::StampedTransform(
tf_frame,
ros::Time::now(),
tf_world_frame_,
link_name));

// Broadcast all male mate points for this atom
for(std::vector<MatePointPtr>::iterator it_mmp = female_atom->male_mate_points.begin();
it_mmp != female_atom->male_mate_points.end();
++it_mmp)
{
MatePointPtr male_mate_point = *it_mmp;

const std::string male_mate_point_name = str(
boost::format("%s/male_%d")
% atom_name
% male_mate_point->model->id);

tf::poseKDLToTF(male_mate_point->model->pose,tf_frame);
br.sendTransform(
tf::StampedTransform(
tf_frame,
ros::Time::now(),
link_name,
male_mate_point_name));

visualization_msgs::Marker mate_marker;
mate_marker.header.frame_id = male_mate_point_name;
mate_marker.header.stamp = ros::Time(0);
mate_marker.type = mate_marker.CUBE;
mate_marker.action = mate_marker.ADD;
mate_marker.id = (iter * 100) + male_mate_point->model->id;
mate_marker.scale.x = 0.02;
mate_marker.scale.y = 0.02;
mate_marker.scale.z = 0.01;
mate_marker.color.r = 1.0;
mate_marker.color.g = 0.0;
mate_marker.color.b = 0.0;
mate_marker.color.a = 0.25;
male_mate_markers.markers.push_back(mate_marker);
}

// Broadcast all female mate points for this atom
for(std::vector<MatePointPtr>::iterator it_fmp = female_atom->female_mate_points.begin();
it_fmp != female_atom->female_mate_points.end();
++it_fmp)
{
MatePointPtr female_mate_point = *it_fmp;

const std::string female_mate_point_name = str(
boost::format("%s/female_%d")
% atom_name
% female_mate_point->model->id);

tf::poseKDLToTF(female_mate_point->model->pose, tf_frame);
br.sendTransform(
tf::StampedTransform(
tf_frame,
ros::Time::now(),
link_name,
female_mate_point_name));

visualization_msgs::Marker mate_marker;
mate_marker.header.frame_id = female_mate_point_name;
mate_marker.header.stamp = ros::Time(0);
mate_marker.type = mate_marker.CUBE;
mate_marker.action = mate_marker.ADD;
mate_marker.id = (iter * 100) + female_mate_point->model->id;
mate_marker.scale.x = 0.02;
mate_marker.scale.y = 0.02;
mate_marker.scale.z = 0.01;
mate_marker.color.r = 0.0;
mate_marker.color.g = 0.0;
mate_marker.color.b = 1.0;
mate_marker.color.a = 0.25;
female_mate_markers.markers.push_back(mate_marker);
}

male_mate_pub_.publish(male_mate_markers);
female_mate_pub_.publish(female_mate_markers);
}
}

// TODO: move this introspection out of this thread
if (publish_active_mates_) {
active_mates_pub_.publish(mates_msg);
}
Expand All @@ -710,22 +719,20 @@ namespace assembly_sim

std::cout << "Collision thread running!" << std::endl;

while (running_) {
gazebo::physics::WorldPtr world = this->model_->GetWorld();
gazebo::common::Time now(0);
gazebo::common::Time update_period(1.0/updates_per_second_);
gazebo::common::Time last_update_time = world->GetSimTime();

clock_t current_tick = clock();
while(running_) {

if ((float)(current_tick - last_tick_) / CLOCKS_PER_SEC > 1.0 / updates_per_second_) {

update_mutex_.lock();

// clear mates
mates_to_attach.clear();
mates_to_detach.clear();
now = world->GetSimTime();

if(now < last_update_time + update_period) {
gazebo::common::Time::Sleep(last_update_time + update_period - now);
} else {
last_update_time = world->GetSimTime();
DoProximityCheck();
update_mutex_.unlock();

last_tick_ = clock();
}
}
}
Expand All @@ -745,18 +752,22 @@ namespace assembly_sim
std::cout << "Started." <<std::endl;
}

if (update_mutex_.try_lock()) {
boost::mutex::scoped_lock update_lock(update_mutex_, boost::try_to_lock);
if (update_lock) {

for (std::vector<MatePtr>::iterator it = mates_to_detach.begin();
// Detach joints
for (boost::unordered_set<MatePtr>::iterator it = mates_to_detach.begin();
it != mates_to_detach.end();
++it)
{
MatePtr mate = *it;
gzwarn<<" --- demating "<<mate->joint->GetName()<<std::endl;
// Detach joint
mate->joint->Detach();
}
for (std::vector<MatePtr>::iterator it = mates_to_attach.begin();
mates_to_detach.clear();

// Attach joints
for (boost::unordered_set<MatePtr>::iterator it = mates_to_attach.begin();
it != mates_to_attach.end();
++it)
{
Expand Down Expand Up @@ -818,11 +829,7 @@ namespace assembly_sim
//gzwarn<<" ---- actual anchor pose: "<<std::endl<<actual_anchor_frame<<std::endl;
gzwarn<<" ---- mate error: "<<mate->mate_error.vel.Norm()<<", "<<mate->mate_error.rot.Norm()<<std::endl;
}

mates_to_attach.clear();
mates_to_detach.clear();

update_mutex_.unlock();
}
}

Expand Down
4 changes: 2 additions & 2 deletions assembly_sim/src/assembly_soup_plugin.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -169,8 +169,8 @@ namespace assembly_sim {
bool running_;

// mates to attach/detach in OnUpdate thread
std::vector<MatePtr> mates_to_attach;
std::vector<MatePtr> mates_to_detach;
boost::unordered_set<MatePtr> mates_to_attach;
boost::unordered_set<MatePtr> mates_to_detach;

protected:
size_t mate_id_counter_;
Expand Down

0 comments on commit 4830f30

Please sign in to comment.