Skip to content

Commit

Permalink
Add ability to disable links when launching contact monitor
Browse files Browse the repository at this point in the history
  • Loading branch information
@Levi-Armstrong
Levi-Armstrong committed Dec 16, 2021
1 parent 5c6b683 commit 11f0c70
Show file tree
Hide file tree
Showing 4 changed files with 89 additions and 22 deletions.
2 changes: 2 additions & 0 deletions tesseract_monitoring/include/tesseract_monitoring/contact_monitor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,7 @@ class ContactMonitor
ros::NodeHandle& nh,
ros::NodeHandle& pnh,
const std::vector<std::string>& monitored_link_names,
const std::vector<std::string>& disabled_link_names,
const tesseract_collision::ContactTestType& type,
double contact_distance = 0.1,
const std::string& joint_state_topic = DEFAULT_JOINT_STATES_TOPIC);
Expand Down Expand Up @@ -112,6 +113,7 @@ class ContactMonitor
ros::NodeHandle& nh_;
ros::NodeHandle& pnh_;
std::vector<std::string> monitored_link_names_;
std::vector<std::string> disabled_link_names_;
tesseract_collision::ContactTestType type_;
double contact_distance_;
tesseract_collision::DiscreteContactManager::UPtr manager_;
Expand Down
4 changes: 3 additions & 1 deletion tesseract_monitoring/launch/contact_monitor.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
<arg name="discrete_plugin" default="BulletDiscreteBVHManager"/>
<arg name="continuous_plugin" default="BulletCastBVHManager"/>
<arg name="monitor_links" default=""/>
<arg name="disabled_links" default=""/>
<arg name="publish_environment" default="false"/>
<arg name="publish_markers" default="false"/>

Expand All @@ -18,12 +19,13 @@
<arg name="contact_test_type" default="2"/>
<arg name="contact_distance"/>

<node pkg="tesseract_monitoring" type="tesseract_monitoring_contacts_node" name="tesseract_monitoring_contacts">
<node pkg="tesseract_monitoring" type="tesseract_monitoring_contacts_node" name="tesseract_monitoring_contacts" output="screen">
<param name="robot_description" type="string" value="$(arg robot_description)"/>
<param name="discrete_plugin" type="string" value="$(arg discrete_plugin)"/>
<param name="continuous_plugin" type="string" value="$(arg continuous_plugin)"/>
<param name="contact_distance" value="$(arg contact_distance)"/>
<param name="monitor_links" value="$(arg monitor_links)"/>
<param name="disabled_links" value="$(arg disabled_links)"/>
<param name="contact_test_type" value="$(arg contact_test_type)"/>
<param name="monitor_namespace" type="string" value="$(arg monitor_namespace)"/>
<param name="monitored_namespace" type="string" value="$(arg monitored_namespace)"/>
Expand Down
48 changes: 31 additions & 17 deletions tesseract_monitoring/src/contact_monitor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,13 +44,15 @@ ContactMonitor::ContactMonitor(std::string monitor_namespace,
ros::NodeHandle& nh,
ros::NodeHandle& pnh,
const std::vector<std::string>& monitored_link_names,
const std::vector<std::string>& disabled_link_names,
const tesseract_collision::ContactTestType& type,
double contact_distance,
const std::string& joint_state_topic)
: monitor_namespace_(std::move(monitor_namespace))
, nh_(nh)
, pnh_(pnh)
, monitored_link_names_(monitored_link_names)
, disabled_link_names_(disabled_link_names)
, type_(type)
, contact_distance_(contact_distance)
{
Expand All @@ -70,6 +72,10 @@ ContactMonitor::ContactMonitor(std::string monitor_namespace,

manager_->setActiveCollisionObjects(monitored_link_names);
manager_->setDefaultCollisionMarginData(contact_distance);
for (const auto& disabled_link_name : disabled_link_names_)
manager_->disableCollisionObject(disabled_link_name);

std::cout << ((disabled_link_names_.empty()) ? "Empty" : "Not Empty") << std::endl;

joint_states_sub_ = nh_.subscribe(joint_state_topic, 1, &ContactMonitor::callbackJointState, this);
std::string contact_results_topic = R"(/)" + monitor_namespace_ + DEFAULT_PUBLISH_CONTACT_RESULTS_TOPIC;
Expand Down Expand Up @@ -127,6 +133,8 @@ void ContactMonitor::computeCollisionReportThread()
manager_->setActiveCollisionObjects(active);
manager_->setCollisionMarginData(contact_margin_data);
manager_->setIsContactAllowedFn(fn);
for (const auto& disabled_link_name : disabled_link_names_)
manager_->disableCollisionObject(disabled_link_name);
}

if (!current_joint_states_)
Expand All @@ -151,25 +159,29 @@ void ContactMonitor::computeCollisionReportThread()
manager_->contactTest(contacts, type_);
}

tesseract_collision::ContactResultVector contacts_vector;
tesseract_collision::flattenResults(std::move(contacts), contacts_vector);
contacts_msg.contacts.reserve(contacts_vector.size());
for (std::size_t i = 0; i < contacts_vector.size(); ++i)
if (!contacts.empty())
{
tesseract_msgs::ContactResult contact_msg;
tesseract_rosutils::toMsg(contact_msg, contacts_vector[i], msg->header.stamp);
contacts_msg.contacts.push_back(contact_msg);
}
contact_results_pub_.publish(contacts_msg);
tesseract_collision::ContactResultVector contacts_vector;
tesseract_collision::flattenResults(std::move(contacts), contacts_vector);
contacts_msg.contacts.reserve(contacts_vector.size());
for (std::size_t i = 0; i < contacts_vector.size(); ++i)
{
tesseract_msgs::ContactResult contact_msg;
tesseract_rosutils::toMsg(contact_msg, contacts_vector[i], msg->header.stamp);
contacts_msg.contacts.push_back(contact_msg);
}

if (publish_contact_markers_)
{
int id_counter = 0;
tesseract_visualization::ContactResultsMarker marker(
monitored_link_names_, contacts_vector, manager_->getCollisionMarginData());
visualization_msgs::MarkerArray marker_msg = tesseract_rosutils::ROSPlotting::getContactResultsMarkerArrayMsg(
id_counter, root_link, "contact_monitor", msg->header.stamp, marker);
contact_marker_pub_.publish(marker_msg);
contact_results_pub_.publish(contacts_msg);

if (publish_contact_markers_)
{
int id_counter = 0;
tesseract_visualization::ContactResultsMarker marker(
monitored_link_names_, contacts_vector, manager_->getCollisionMarginData());
visualization_msgs::MarkerArray marker_msg = tesseract_rosutils::ROSPlotting::getContactResultsMarkerArrayMsg(
id_counter, root_link, "contact_monitor", msg->header.stamp, marker);
contact_marker_pub_.publish(marker_msg);
}
}
}
}
Expand Down Expand Up @@ -207,6 +219,8 @@ bool ContactMonitor::callbackModifyTesseractEnv(tesseract_msgs::ModifyEnvironmen
manager_->setActiveCollisionObjects(active);
manager_->setCollisionMarginData(contact_margin_data);
manager_->setIsContactAllowedFn(fn);
for (const auto& disabled_link_name : disabled_link_names_)
manager_->disableCollisionObject(disabled_link_name);

return true;
}
Expand Down
57 changes: 53 additions & 4 deletions tesseract_monitoring/src/contact_monitor_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -99,13 +99,55 @@ int main(int argc, char** argv)
pnh.param<double>("contact_distance", contact_distance, DEFAULT_CONTACT_DISTANCE);

std::vector<std::string> monitored_link_names;
monitored_link_names = env->getLinkNames();
if (pnh.hasParam("monitor_links"))
pnh.getParam("monitor_links", monitored_link_names);
{
std::string monitored_link_names_str;
pnh.getParam("monitor_links", monitored_link_names_str);
if (!monitored_link_names_str.empty())
boost::split(monitored_link_names, monitored_link_names_str, boost::is_any_of(" "));
}

if (monitored_link_names.empty())
monitored_link_names = env->getLinkNames();

std::vector<std::string> disabled_link_names;
if (pnh.hasParam("disabled_links"))
{
std::string disabled_link_names_str;
pnh.getParam("disabled_links", disabled_link_names_str);
if (!disabled_link_names_str.empty())
boost::split(disabled_link_names, disabled_link_names_str, boost::is_any_of(" "));
}

std::string disabled_link_names_str;
for (const auto& disabled_link : disabled_link_names)
{
if (disabled_link_names_str.empty())
disabled_link_names_str = disabled_link;
else
disabled_link_names_str += (", " + disabled_link);
}

ROS_INFO("DISABLED_LINKS: %s", disabled_link_names_str.c_str());

auto pred = [&disabled_link_names](const std::string& key) -> bool {
return std::find(disabled_link_names.begin(), disabled_link_names.end(), key) != disabled_link_names.end();
};

monitored_link_names.erase(std::remove_if(monitored_link_names.begin(), monitored_link_names.end(), pred),
monitored_link_names.end());

std::string monitored_link_names_str;
for (const auto& monitor_link : monitored_link_names)
{
if (monitored_link_names_str.empty())
monitored_link_names_str = monitor_link;
else
monitored_link_names_str += (", " + monitor_link);
}

ROS_INFO("MONITORED_LINKS: %s", monitored_link_names_str.c_str());

int contact_test_type = 2;
if (pnh.hasParam("contact_test_type"))
pnh.getParam("contact_test_type", contact_test_type);
Expand All @@ -117,8 +159,15 @@ int main(int argc, char** argv)
}
tesseract_collision::ContactTestType type = static_cast<tesseract_collision::ContactTestType>(contact_test_type);

tesseract_monitoring::ContactMonitor cm(
monitor_namespace, std::move(env), nh, pnh, monitored_link_names, type, contact_distance, joint_state_topic);
tesseract_monitoring::ContactMonitor cm(monitor_namespace,
std::move(env),
nh,
pnh,
monitored_link_names,
disabled_link_names,
type,
contact_distance,
joint_state_topic);

if (publish_environment)
cm.startPublishingEnvironment();
Expand Down

0 comments on commit 11f0c70

Please sign in to comment.