diff --git a/common/ros_observer/CMakeLists.txt b/common/ros_observer/CMakeLists.txt new file mode 100644 index 00000000000..05811734da3 --- /dev/null +++ b/common/ros_observer/CMakeLists.txt @@ -0,0 +1,93 @@ +cmake_minimum_required(VERSION 3.1) +project(ros_observer) + +find_package(carma_cmake_common REQUIRED) +carma_check_ros_version(1) + +set(CMAKE_CXX_STANDARD 14) + +find_package( + catkin REQUIRED COMPONENTS + roscpp + roslint +) + +find_package(Boost REQUIRED COMPONENTS + thread +) + +set(ROSLINT_CPP_OPTS "--filter=-build/c++14") +roslint_cpp() + +catkin_package( + INCLUDE_DIRS include + LIBRARIES lib_ros_observer +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +add_executable( + ros_observer + src/ros_observer.cpp +) + +target_link_libraries( + ros_observer + rt ${Boost_LIBRARIES} + pthread +) + +add_dependencies( + ros_observer + ${catkin_EXPORTED_TARGETS} +) + +add_library( + lib_ros_observer + lib/lib_ros_observer.cpp +) + +target_link_libraries( + lib_ros_observer + ${catkin_LIBRARIES} + rt + ${Boost_LIBRARIES} +) + +add_dependencies( + lib_ros_observer + ${catkin_EXPORTED_TARGETS} +) + +# include header files +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +# Install library +install(TARGETS ros_observer lib_ros_observer + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# Install launch +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +) + +if (CATKIN_ENABLE_TESTING) + roslint_add_test() + find_package(rostest REQUIRED) + add_rostest_gtest(test_ros_observer + test/test_ros_observer.test + test/src/test_ros_observer.cpp + ) + target_link_libraries(test_ros_observer + lib_ros_observer + ${catkin_LIBRARIES} + ) +endif() diff --git a/common/ros_observer/include/ros_observer/lib_ros_observer.h b/common/ros_observer/include/ros_observer/lib_ros_observer.h new file mode 100644 index 00000000000..37f49de182e --- /dev/null +++ b/common/ros_observer/include/ros_observer/lib_ros_observer.h @@ -0,0 +1,71 @@ +#ifndef ROS_OBSERVER_LIB_ROS_OBSERVER_H +#define ROS_OBSERVER_LIB_ROS_OBSERVER_H + +/* + * Copyright 2019 Autoware Foundation. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include +#include +#include +#include + +#include + +enum class VitalMonitorMode +{ + CNT_CLEAR, + CNT_MON +}; + +class ShmVitalMonitor +{ +public: + ShmVitalMonitor(std::string mod_name, const double loop_rate, VitalMonitorMode mode = VitalMonitorMode::CNT_CLEAR); + + void set_mode(VitalMonitorMode mode) {mode_ = mode;} + void run(void); + bool is_error_detected(void); + +protected: + std::string name_, shm_name_, mut_name_; + VitalMonitorMode mode_; + bool is_opened_; + const unsigned int polling_interval_msec_; + + bool attempt_to_open(void); + void init_vital_counter(void); + void update_vital_counter(void); +}; + +class ShmDRStopRequest +{ +public: + ShmDRStopRequest() : + is_opened_(false), name_("DRStopRequest"), shm_name_("SHM_" + name_), mut_name_("MUT_" + name_) {} + + void clear_request(void); + bool is_request_received(void); + +protected: + bool is_opened_; + std::string name_, shm_name_, mut_name_; + + bool attempt_to_open(void); +}; + +#endif // ROS_OBSERVER_LIB_ROS_OBSERVER_H diff --git a/common/ros_observer/include/ros_observer/ros_observer.h b/common/ros_observer/include/ros_observer/ros_observer.h new file mode 100644 index 00000000000..1f5d7b05c8a --- /dev/null +++ b/common/ros_observer/include/ros_observer/ros_observer.h @@ -0,0 +1,53 @@ +#ifndef ROS_OBSERVER_ROS_OBSERVER_H +#define ROS_OBSERVER_ROS_OBSERVER_H + +/* + * Copyright 2019 Autoware Foundation. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +static constexpr const char* SHM_NAME = "SharedMemoryForVitalMonitor"; +static constexpr int SHM_SIZE = 65536; +static constexpr unsigned int SHM_TH_COUNTER = 3; +static constexpr unsigned int SHM_COUNTER_MAX = 10000; + +enum class ModuleStatus +{ + Normal, + ErrorDetected +}; + +struct ShmVitalCounter +{ + ModuleStatus modstatus; + + bool activated; + unsigned int thresh; + unsigned int value; + + ShmVitalCounter() : + modstatus(ModuleStatus::Normal), activated(false), thresh(0), value(0) {} +}; + +#endif // ROS_OBSERVER_ROS_OBSERVER_H diff --git a/common/ros_observer/launch/ros_observer.launch b/common/ros_observer/launch/ros_observer.launch new file mode 100644 index 00000000000..a91f587815a --- /dev/null +++ b/common/ros_observer/launch/ros_observer.launch @@ -0,0 +1,4 @@ + + + + diff --git a/common/ros_observer/launch/test.launch b/common/ros_observer/launch/test.launch new file mode 100644 index 00000000000..45ae3841637 --- /dev/null +++ b/common/ros_observer/launch/test.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/common/ros_observer/lib/lib_ros_observer.cpp b/common/ros_observer/lib/lib_ros_observer.cpp new file mode 100644 index 00000000000..5f286c52504 --- /dev/null +++ b/common/ros_observer/lib/lib_ros_observer.cpp @@ -0,0 +1,207 @@ +/* + * Copyright 2019 Autoware Foundation. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include +#include +#include +#include + +using boost::interprocess::managed_shared_memory; +using boost::interprocess::scoped_lock; +using boost::interprocess::open_only; +using boost::interprocess::interprocess_mutex; +using boost::interprocess::interprocess_exception; + +ShmVitalMonitor::ShmVitalMonitor(std::string mod_name, const double loop_rate, VitalMonitorMode mode) : + is_opened_(false), name_(mod_name), shm_name_("SHM_" + mod_name), mut_name_("MUT_" + mod_name), + mode_(mode), polling_interval_msec_(1000.0/loop_rate) {} + +void ShmVitalMonitor::run(void) +{ + if (is_opened_) + { + update_vital_counter(); + } + else + { + is_opened_ = attempt_to_open(); + if (is_opened_) init_vital_counter(); + } +} + +void ShmVitalMonitor::init_vital_counter(void) +{ + if (mode_ == VitalMonitorMode::CNT_CLEAR) + { + try + { + managed_shared_memory shm(open_only, SHM_NAME); + ShmVitalCounter* p_cnt = shm.find(shm_name_.c_str()).first; + interprocess_mutex* p_mut = shm.find(mut_name_.c_str()).first; + scoped_lock scpdlock(*p_mut); + + p_cnt->activated = true; + p_cnt->thresh = (polling_interval_msec_)*(SHM_TH_COUNTER); + p_cnt->value = 0; + } + catch(interprocess_exception &ex) + { + std::cout << "[INFO][Failed to connect shared memory]" << std::endl; + } + } +} + +void ShmVitalMonitor::update_vital_counter(void) +{ + try + { + managed_shared_memory shm(open_only, SHM_NAME); + ShmVitalCounter* p_cnt = shm.find(shm_name_.c_str()).first; + interprocess_mutex* p_mut = shm.find(mut_name_.c_str()).first; + scoped_lock scpdlock(*p_mut); + + if (mode_ == VitalMonitorMode::CNT_CLEAR) + { + p_cnt->value = 0; + } + else if (mode_ == VitalMonitorMode::CNT_MON) + { + p_cnt->value = (p_cnt->activated) ? std::min((p_cnt->value + polling_interval_msec_), SHM_COUNTER_MAX) : 0; + p_cnt->modstatus = (p_cnt->value > p_cnt->thresh) ? ModuleStatus::ErrorDetected : ModuleStatus::Normal; + } + } + catch(interprocess_exception &ex) + { + std::cout << "[INFO][Failed to connect shared memory]" << std::endl; + } +} + +bool ShmVitalMonitor::attempt_to_open(void) +{ + bool is_opened = false; + + try + { + managed_shared_memory shm(open_only, SHM_NAME); + ShmVitalCounter* p_cnt = shm.find(shm_name_.c_str()).first; + interprocess_mutex* p_mut = shm.find(mut_name_.c_str()).first; + is_opened = true; + } + catch(interprocess_exception &ex) + { + is_opened = false; + } + return is_opened; +} + +bool ShmVitalMonitor::is_error_detected(void) +{ + bool is_error_detected = false; + + if (!is_opened_) + { + is_opened_ = attempt_to_open(); + } + else + { + try + { + managed_shared_memory shm(open_only, SHM_NAME); + ShmVitalCounter* p_cnt = shm.find(shm_name_.c_str()).first; + interprocess_mutex* p_mut = shm.find(mut_name_.c_str()).first; + + scoped_lock scpdlock(*p_mut); + is_error_detected = (p_cnt->modstatus == ModuleStatus::ErrorDetected) ? true : false; + } + catch(interprocess_exception &ex) + { + is_error_detected = true; + } + } + return is_error_detected; +} + +bool ShmDRStopRequest::is_request_received(void) +{ + bool is_request_received = false; + + if (!is_opened_) + { + is_opened_ = attempt_to_open(); + } + else + { + try + { + managed_shared_memory shm(open_only, SHM_NAME); + bool* p_stop_request = shm.find(shm_name_.c_str()).first; + interprocess_mutex* p_mut = shm.find(mut_name_.c_str()).first; + + scoped_lock scpdlock(*p_mut); + is_request_received = (*p_stop_request); + } + catch(interprocess_exception &ex) + { + std::cout << "[INFO][Failed to connect shared memory]" << std::endl; + is_request_received = false; + } + } + return is_request_received; +} + +void ShmDRStopRequest::clear_request(void) +{ + if (!is_opened_) + { + is_opened_ = attempt_to_open(); + } + else + { + try + { + managed_shared_memory shm(open_only, SHM_NAME); + bool* p_stop_request = shm.find(shm_name_.c_str()).first; + interprocess_mutex* p_mut = shm.find(mut_name_.c_str()).first; + + scoped_lock scpdlock(*p_mut); + (*p_stop_request) = false; + } + catch(interprocess_exception &ex) + { + std::cout << "[INFO][Failed to connect shared memory]" << std::endl; + } + } +} + +bool ShmDRStopRequest::attempt_to_open(void) +{ + bool is_opened = false; + + try + { + managed_shared_memory shm(open_only, SHM_NAME); + bool* p_stop_request = shm.find(shm_name_.c_str()).first; + interprocess_mutex* p_mut = shm.find(mut_name_.c_str()).first; + is_opened = true; + } + catch(interprocess_exception &ex) + { + is_opened = false; + } + return is_opened; +} diff --git a/common/ros_observer/package.xml b/common/ros_observer/package.xml new file mode 100644 index 00000000000..14a9c09370b --- /dev/null +++ b/common/ros_observer/package.xml @@ -0,0 +1,17 @@ + + + ros_observer + 1.12.0 + The ros_observer package + Shingo Takeuchi + Shingo Takeuchi + Apache 2 + catkin + rostest + roscpp + boost + roslint + carma_cmake_common + + + diff --git a/common/ros_observer/src/ros_observer.cpp b/common/ros_observer/src/ros_observer.cpp new file mode 100644 index 00000000000..e398650971c --- /dev/null +++ b/common/ros_observer/src/ros_observer.cpp @@ -0,0 +1,193 @@ +/* + * Copyright 2019 Autoware Foundation. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include +#include +#include +#include + +using boost::interprocess::managed_shared_memory; +using boost::interprocess::interprocess_mutex; +using boost::interprocess::shared_memory_object; +using boost::interprocess::scoped_lock; +using boost::interprocess::create_only; + +static void sig_handler_init(void); +static void sig_handler(void); + +static constexpr double ROS_OBSERVE_MONITOR_RATE = 100; +static constexpr unsigned int POLLING_INTERVAL_MSEC = (1000.0 / ROS_OBSERVE_MONITOR_RATE); +static constexpr unsigned int POLLING_INTERVAL_USEC = (POLLING_INTERVAL_MSEC * 1000); +static constexpr unsigned int SHM_TH_COUNTER_RO = 10; + +static bool terminate_req_rcvd = false; + +// Signal Handler +static void sig_handler(int signo) +{ + terminate_req_rcvd = true; +} + +// Signal Handler Initialization +static void sig_handler_init(void) +{ + if (signal(SIGUSR1, sig_handler) == SIG_ERR) + { + std::cout << "[INFO][Cannot catch SIGUSR1]" << std::endl; + } + + if (signal(SIGTERM, sig_handler) == SIG_ERR) + { + std::cout << "[INFO][Cannot catch SIGTERM]" << std::endl; + } + + if (signal(SIGINT, sig_handler) == SIG_ERR) + { + std::cout << "[INFO][Cannot catch SIGINT]" << std::endl; + } + + if (signal(SIGQUIT, sig_handler) == SIG_ERR) + { + std::cout << "[INFO][Cannot catch SIGQUIT]" << std::endl; + } +} + +int main(int argc, char* argv[]) +{ + sig_handler_init(); + + struct tm localtime; + auto now = std::chrono::system_clock::now(); + auto now_c = std::chrono::system_clock::to_time_t(now); + localtime_r(&now_c, &localtime); + std::cout << "[START][TIME][LOCAL: " << std::put_time(&localtime, "%c") << "]" << std::endl; + + shared_memory_object::remove(SHM_NAME); + managed_shared_memory shm(create_only, SHM_NAME, SHM_SIZE); + + ShmVitalCounter* p_cnt_RO = shm.construct("SHM_RosObserver")(); + ShmVitalCounter* p_cnt_HA = shm.construct("SHM_HealthAggregator")(); + ShmVitalCounter* p_cnt_EH = shm.construct("SHM_EmergencyHandler")(); + ShmVitalCounter* p_cnt_TG = shm.construct("SHM_TwistGate")(); + ShmVitalCounter* p_cnt_YMC = shm.construct("SHM_YMC_VehicleDriver")(); + ShmVitalCounter* p_cnt_AS = shm.construct("SHM_AS_VehicleDriver")(); + bool* p_stopReq_DR = shm.construct("SHM_DRStopRequest")(); + + interprocess_mutex* p_mut_RO = shm.construct("MUT_RosObserver")(); + interprocess_mutex* p_mut_HA = shm.construct("MUT_HealthAggregator")(); + interprocess_mutex* p_mut_EH = shm.construct("MUT_EmergencyHandler")(); + interprocess_mutex* p_mut_TG = shm.construct("MUT_TwistGate")(); + interprocess_mutex* p_mut_YMC = shm.construct("MUT_YMC_VehicleDriver")(); + interprocess_mutex* p_mut_AS = shm.construct("MUT_AS_VehicleDriver")(); + interprocess_mutex* p_mut_DR = shm.construct("MUT_DRStopRequest")(); + + { + scoped_lock scpdlock_RO(*p_mut_RO); + p_cnt_RO->activated = true; + p_cnt_RO->thresh = (POLLING_INTERVAL_MSEC) * (SHM_TH_COUNTER_RO); + p_cnt_RO->value = 0; + } + + while (!terminate_req_rcvd) + { + { + scoped_lock scpdlock_RO(*p_mut_RO); + scoped_lock scpdlock_HA(*p_mut_HA); + scoped_lock scpdlock_EH(*p_mut_EH); + scoped_lock scpdlock_TG(*p_mut_TG); + scoped_lock scpdlock_YMC(*p_mut_YMC); + scoped_lock scpdlock_AS(*p_mut_AS); + scoped_lock scpdlock_DR(*p_mut_DR); + + p_cnt_RO->value = 0; + p_cnt_HA->value = + (p_cnt_HA->activated) ? std::min((p_cnt_HA->value + (POLLING_INTERVAL_MSEC)), SHM_COUNTER_MAX) : 0; + p_cnt_EH->value = + (p_cnt_EH->activated) ? std::min((p_cnt_EH->value + (POLLING_INTERVAL_MSEC)), SHM_COUNTER_MAX) : 0; + p_cnt_TG->value = + (p_cnt_TG->activated) ? std::min((p_cnt_TG->value + (POLLING_INTERVAL_MSEC)), SHM_COUNTER_MAX) : 0; + p_cnt_YMC->value = + (p_cnt_YMC->activated) ? std::min((p_cnt_YMC->value + (POLLING_INTERVAL_MSEC)), SHM_COUNTER_MAX) : 0; + p_cnt_AS->value = + (p_cnt_AS->activated) ? std::min((p_cnt_AS->value + (POLLING_INTERVAL_MSEC)), SHM_COUNTER_MAX) : 0; + + static bool ros_error_detected_prev = false; + bool ros_error_detected = false; + std::string error_node; + + if (p_cnt_HA->value > p_cnt_HA->thresh) + { + ros_error_detected = true; + error_node = "Health Aggregator"; + } + + if (p_cnt_EH->value > p_cnt_EH->thresh) + { + ros_error_detected = true; + error_node = "Emergency Handler"; + } + + if (p_cnt_TG->value > p_cnt_TG->thresh) + { + ros_error_detected = true; + error_node = "Twist Gate"; + } + + if (p_cnt_YMC->value > p_cnt_YMC->thresh) + { + ros_error_detected = true; + error_node = "YMC Vehicle Driver"; + } + + if (p_cnt_AS->value > p_cnt_AS->thresh) + { + ros_error_detected = true; + error_node = "AS Vehicle Driver"; + } + + if (ros_error_detected) + { + p_cnt_HA->modstatus = ModuleStatus::ErrorDetected; + if (!ros_error_detected_prev) + { + (*p_stopReq_DR) = true; + } + + auto now = std::chrono::system_clock::now(); + auto now_c = std::chrono::system_clock::to_time_t(now); + localtime_r(&now_c, &localtime); + std::cerr << "[START][TIME][LOCAL: " << std::put_time(&localtime, "%c") << "][" << error_node.c_str() << "]" + << std::endl; + } + else + { + p_cnt_HA->modstatus = ModuleStatus::Normal; + if (ros_error_detected_prev) + { + (*p_stopReq_DR) = false; + } + } + ros_error_detected_prev = ros_error_detected; + } + usleep(POLLING_INTERVAL_USEC); + } + + shared_memory_object::remove(SHM_NAME); + + return 0; +} diff --git a/common/ros_observer/test/src/test_ros_observer.cpp b/common/ros_observer/test/src/test_ros_observer.cpp new file mode 100644 index 00000000000..88f00050a48 --- /dev/null +++ b/common/ros_observer/test/src/test_ros_observer.cpp @@ -0,0 +1,258 @@ +/* + * Copyright 2019 Autoware Foundation. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include +#include +#include +#include + +using boost::interprocess::managed_shared_memory; +using boost::interprocess::shared_memory_object; +using boost::interprocess::scoped_lock; +using boost::interprocess::create_only; +using boost::interprocess::interprocess_mutex; + +class MyShmDRStopRequest : public ShmDRStopRequest +{ + friend class ShmTestSuite; + +public: + MyShmDRStopRequest(): ShmDRStopRequest() {} +}; + +class MyShmVitalMonitor : public ShmVitalMonitor +{ + friend class ShmTestSuite; + +public: + MyShmVitalMonitor(std::string name, const double rate, VitalMonitorMode mode = VitalMonitorMode::CNT_CLEAR): + ShmVitalMonitor(name, rate, mode) {} +}; + +class ShmTestSuite : public ::testing::Test +{ +protected: + std::unique_ptr myVMObj_; + std::unique_ptr myDRObj_; + + void setupVM(std::string name, const double rate, VitalMonitorMode mode = VitalMonitorMode::CNT_CLEAR) + { + myVMObj_ = std::unique_ptr(new MyShmVitalMonitor(name, rate, mode)); + } + + void setupDR(void) + { + myDRObj_ = std::unique_ptr(new MyShmDRStopRequest()); + } + +public: + void modeTestVM(void) + { + ASSERT_EQ(myVMObj_->mode_, VitalMonitorMode::CNT_CLEAR); + myVMObj_->set_mode(VitalMonitorMode::CNT_MON); + ASSERT_EQ(myVMObj_->mode_, VitalMonitorMode::CNT_MON); + } + + void nameTestVM(void) + { + ASSERT_EQ(myVMObj_->shm_name_, "SHM_NameTest"); + ASSERT_EQ(myVMObj_->mut_name_, "MUT_NameTest"); + } + + void openTestVM(void) + { + EXPECT_FALSE(myVMObj_->attempt_to_open()); + + managed_shared_memory shm_new(create_only, SHM_NAME, SHM_SIZE); + ShmVitalCounter* p_cnt_new = shm_new.construct(myVMObj_->shm_name_.c_str())(); + interprocess_mutex* p_mut_new = shm_new.construct(myVMObj_->mut_name_.c_str())(); + + ASSERT_TRUE(myVMObj_->attempt_to_open()); + + shared_memory_object::remove(SHM_NAME); + } + + void runTestVM(void) + { + managed_shared_memory shm_new(create_only, SHM_NAME, SHM_SIZE); + ShmVitalCounter* p_cnt_new = shm_new.construct(myVMObj_->shm_name_.c_str())(); + interprocess_mutex* p_mut_new = shm_new.construct(myVMObj_->mut_name_.c_str())(); + + myVMObj_->run(); + ASSERT_TRUE(p_cnt_new->activated); + + shared_memory_object::remove(SHM_NAME); + } + + void updateTestVM(void) + { + managed_shared_memory shm_new(create_only, SHM_NAME, SHM_SIZE); + ShmVitalCounter* p_cnt_new = shm_new.construct(myVMObj_->shm_name_.c_str())(); + interprocess_mutex* p_mut_new = shm_new.construct(myVMObj_->mut_name_.c_str())(); + + myVMObj_->mode_ = VitalMonitorMode::CNT_CLEAR; + p_cnt_new->activated = true; + p_cnt_new->value = 100; + myVMObj_->update_vital_counter(); + ASSERT_EQ(p_cnt_new->value, 0); + + myVMObj_->mode_ = VitalMonitorMode::CNT_MON; + myVMObj_->update_vital_counter(); + ASSERT_EQ(p_cnt_new->value, myVMObj_->polling_interval_msec_); + + p_cnt_new->value = SHM_COUNTER_MAX; + myVMObj_->update_vital_counter(); + ASSERT_EQ(p_cnt_new->value, SHM_COUNTER_MAX); + + p_cnt_new->value = p_cnt_new->thresh - 10; /* 100hz */ + myVMObj_->update_vital_counter(); + ASSERT_EQ(p_cnt_new->modstatus, ModuleStatus::Normal); + myVMObj_->update_vital_counter(); + ASSERT_EQ(p_cnt_new->modstatus, ModuleStatus::ErrorDetected); + + shared_memory_object::remove(SHM_NAME); + } + + void errorDetectionTestVM(void) + { + ASSERT_FALSE(myVMObj_->is_opened_); + + managed_shared_memory shm_new(create_only, SHM_NAME, SHM_SIZE); + ShmVitalCounter* p_cnt_new = shm_new.construct(myVMObj_->shm_name_.c_str())(); + interprocess_mutex* p_mut_new = shm_new.construct(myVMObj_->mut_name_.c_str())(); + + myVMObj_->run(); + p_cnt_new->modstatus = ModuleStatus::Normal; + ASSERT_FALSE(myVMObj_->is_error_detected()); + p_cnt_new->modstatus = ModuleStatus::ErrorDetected; + ASSERT_TRUE(myVMObj_->is_error_detected()); + + shared_memory_object::remove(SHM_NAME); + } + + void openTestDR(void) + { + myDRObj_->is_opened_ = myDRObj_->attempt_to_open(); + ASSERT_FALSE(myDRObj_->is_opened_); + + managed_shared_memory shm_new(create_only, SHM_NAME, SHM_SIZE); + bool* p_stop_request_new = shm_new.construct(myDRObj_->shm_name_.c_str())(); + interprocess_mutex* p_mut_new = shm_new.construct(myDRObj_->mut_name_.c_str())(); + + myDRObj_->is_opened_ = myDRObj_->attempt_to_open(); + ASSERT_TRUE(myDRObj_->is_opened_); + + shared_memory_object::remove(SHM_NAME); + } + + void clearTestDR(void) + { + managed_shared_memory shm_new(create_only, SHM_NAME, SHM_SIZE); + bool* p_stop_request_new = shm_new.construct(myDRObj_->shm_name_.c_str())(); + interprocess_mutex* p_mut_new = shm_new.construct(myDRObj_->mut_name_.c_str())(); + + (*p_stop_request_new) = false; + myDRObj_->clear_request(); + ASSERT_FALSE((*p_stop_request_new)); + (*p_stop_request_new) = true; + myDRObj_->clear_request(); + ASSERT_FALSE((*p_stop_request_new)); + + shared_memory_object::remove(SHM_NAME); + } + + void requestCheckTestDR(void) + { + managed_shared_memory shm_new(create_only, SHM_NAME, SHM_SIZE); + bool* p_stop_request_new = shm_new.construct(myDRObj_->shm_name_.c_str())(); + interprocess_mutex* p_mut_new = shm_new.construct(myDRObj_->mut_name_.c_str())(); + bool is_request_received = false; + + (*p_stop_request_new) = false; + is_request_received = myDRObj_->is_request_received(); + ASSERT_FALSE(is_request_received); + (*p_stop_request_new) = true; + is_request_received = myDRObj_->is_request_received(); + ASSERT_TRUE(is_request_received); + + shared_memory_object::remove(SHM_NAME); + } +}; + +TEST_F(ShmTestSuite, ModeTestVM) +{ + setupVM("ModeTest", 100.0); + modeTestVM(); +} + +TEST_F(ShmTestSuite, NameTestVM) +{ + setupVM("NameTest", 100.0); + nameTestVM(); +} + +TEST_F(ShmTestSuite, OpenTestVM) +{ + setupVM("OpenTest", 100.0); + openTestVM(); +} + +TEST_F(ShmTestSuite, RunTestVM) +{ + setupVM("RunTest", 100.0); + runTestVM(); +} + +TEST_F(ShmTestSuite, UpdateTestVM) +{ + setupVM("UpdateTest", 100.0); + updateTestVM(); +} + +TEST_F(ShmTestSuite, ErrorDetectionTestVM) +{ + setupVM("ErrorDetectionTest", 100.0); + errorDetectionTestVM(); +} + +TEST_F(ShmTestSuite, OpenTestDR) +{ + setupDR(); + openTestDR(); +} + +TEST_F(ShmTestSuite, ClearTestDR) +{ + setupDR(); + clearTestDR(); +} + +TEST_F(ShmTestSuite, RequestCheckTestDR) +{ + setupDR(); + requestCheckTestDR(); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "SharedMemoryMonitorTestNode"); + + return RUN_ALL_TESTS(); +} diff --git a/common/ros_observer/test/test_ros_observer.test b/common/ros_observer/test/test_ros_observer.test new file mode 100644 index 00000000000..b3b159bbb64 --- /dev/null +++ b/common/ros_observer/test/test_ros_observer.test @@ -0,0 +1,3 @@ + + +