diff --git a/tesseract_monitoring/include/tesseract_monitoring/contact_monitor.h b/tesseract_monitoring/include/tesseract_monitoring/contact_monitor.h index a01ffa6d7..5cb25010e 100644 --- a/tesseract_monitoring/include/tesseract_monitoring/contact_monitor.h +++ b/tesseract_monitoring/include/tesseract_monitoring/contact_monitor.h @@ -53,6 +53,7 @@ class ContactMonitor ros::NodeHandle& nh, ros::NodeHandle& pnh, const std::vector& monitored_link_names, + const std::vector& disabled_link_names, const tesseract_collision::ContactTestType& type, double contact_distance = 0.1, const std::string& joint_state_topic = DEFAULT_JOINT_STATES_TOPIC); @@ -112,6 +113,7 @@ class ContactMonitor ros::NodeHandle& nh_; ros::NodeHandle& pnh_; std::vector monitored_link_names_; + std::vector disabled_link_names_; tesseract_collision::ContactTestType type_; double contact_distance_; tesseract_collision::DiscreteContactManager::UPtr manager_; diff --git a/tesseract_monitoring/launch/contact_monitor.launch b/tesseract_monitoring/launch/contact_monitor.launch index 3acc2303c..b0e7a2c04 100644 --- a/tesseract_monitoring/launch/contact_monitor.launch +++ b/tesseract_monitoring/launch/contact_monitor.launch @@ -6,6 +6,7 @@ + @@ -18,12 +19,13 @@ - + + diff --git a/tesseract_monitoring/src/contact_monitor.cpp b/tesseract_monitoring/src/contact_monitor.cpp index 62ae8da3e..39f15c24d 100644 --- a/tesseract_monitoring/src/contact_monitor.cpp +++ b/tesseract_monitoring/src/contact_monitor.cpp @@ -44,6 +44,7 @@ ContactMonitor::ContactMonitor(std::string monitor_namespace, ros::NodeHandle& nh, ros::NodeHandle& pnh, const std::vector& monitored_link_names, + const std::vector& disabled_link_names, const tesseract_collision::ContactTestType& type, double contact_distance, const std::string& joint_state_topic) @@ -51,6 +52,7 @@ ContactMonitor::ContactMonitor(std::string monitor_namespace, , nh_(nh) , pnh_(pnh) , monitored_link_names_(monitored_link_names) + , disabled_link_names_(disabled_link_names) , type_(type) , contact_distance_(contact_distance) { @@ -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; @@ -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_) @@ -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); + } } } } @@ -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; } diff --git a/tesseract_monitoring/src/contact_monitor_node.cpp b/tesseract_monitoring/src/contact_monitor_node.cpp index 5da852040..28845398d 100644 --- a/tesseract_monitoring/src/contact_monitor_node.cpp +++ b/tesseract_monitoring/src/contact_monitor_node.cpp @@ -99,13 +99,55 @@ int main(int argc, char** argv) pnh.param("contact_distance", contact_distance, DEFAULT_CONTACT_DISTANCE); std::vector 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 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); @@ -117,8 +159,15 @@ int main(int argc, char** argv) } tesseract_collision::ContactTestType type = static_cast(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();