diff --git a/iop_client_power_plant_fkie/CMakeLists.txt b/iop_client_power_plant_fkie/CMakeLists.txt new file mode 100644 index 0000000..0093e30 --- /dev/null +++ b/iop_client_power_plant_fkie/CMakeLists.txt @@ -0,0 +1,87 @@ +cmake_minimum_required(VERSION 2.8.3) +project(iop_client_power_plant_fkie) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + iop_component_fkie + iop_ocu_slavelib_fkie + roscpp + std_msgs) +iop_init(COMPONENT_ID 0) +iop_export_service(urn_jaus_jss_ugv_PowerPlantManagerClient) + +################################### +## catkin specific configuration ## +################################### +catkin_package( +# INCLUDE_DIRS include/public + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS + iop_component_fkie + iop_ocu_slavelib_fkie + std_msgs +) + +########### +## Build ## +########### + +## Specify additional locations of header files +iop_code_generator( + IDLS + urn.jaus.jss.core-v1.1/AccessControlClient.xml + urn.jaus.jss.core-v1.1/EventsClient.xml + urn.jaus.jss.core-v1.1/Transport.xml + urn.jaus.jss.ugv/power_plantClient.xml + OWN_IDLS + OVERRIDES + include/urn_jaus_jss_ugv_PowerPlantManagerClient/PowerPlantManagerClient_ReceiveFSM.h + src/urn_jaus_jss_ugv_PowerPlantManagerClient/PowerPlantManagerClient_ReceiveFSM.cpp + src/main.cpp + EXTERN_SERVICES + urn_jaus_jss_core_AccessControlClient + urn_jaus_jss_core_EventsClient + urn_jaus_jss_core_Transport + GENERATED_SOURCES cppfiles +) +include_directories(${catkin_INCLUDE_DIRS} +# include/public +) + +## Declare a cpp executable +add_library(${PROJECT_NAME} + src/PowerPlantManagerClientPlugin.cpp + ${cppfiles} +) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +) + +install( + DIRECTORY ${IOP_INSTALL_INCLUDE_DIRS} DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} + PATTERN "*.old" EXCLUDE + PATTERN "*.gen" EXCLUDE +) + +#install( +# DIRECTORY include/public/${PROJECT_NAME} DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} +# PATTERN "*.old" EXCLUDE +# PATTERN "*.gen" EXCLUDE +#) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +install( + FILES ./plugin_iop.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + diff --git a/iop_client_power_plant_fkie/README.md b/iop_client_power_plant_fkie/README.md new file mode 100644 index 0000000..7bf176c --- /dev/null +++ b/iop_client_power_plant_fkie/README.md @@ -0,0 +1,30 @@ +This package is part of [ROS/IOP Bridge](https://github.com/fkie/iop_core/blob/master/README.md). + + +## _iop_client_power_plant_fkie:_ PowerPlantManagerClient + +Currently publish reports the status of one battery. + + +#### Parameter: + +_hz (int_ Default: 0.0) + +> Sets how often the reports are requested. If [use_queries](https://github.com/fkie/iop_core/blob/master/iop_ocu_slavelib_fkie/README.md#parameter) is ```True``` hz must be greather then 0. In this case each time a ```Query``` message is sent to get a report. If ```use_queries``` is ```False``` an event is created to get Reports. In this case 0 disables the rate and an event of type ```on_change``` will be created. + + +#### Publisher: + +_powerplant\_`ID`/voltage (std_msgs::Float32)_ + +> Current voltage. + +_powerplant\_`ID`/capacity_percent (std_msgs::Int8)_ + +> Percent of maximum. + + +#### Subscriber: + +> None + diff --git a/iop_client_power_plant_fkie/include/urn_jaus_jss_ugv_PowerPlantManagerClient/PowerPlantManagerClient_ReceiveFSM.h b/iop_client_power_plant_fkie/include/urn_jaus_jss_ugv_PowerPlantManagerClient/PowerPlantManagerClient_ReceiveFSM.h new file mode 100644 index 0000000..e46a816 --- /dev/null +++ b/iop_client_power_plant_fkie/include/urn_jaus_jss_ugv_PowerPlantManagerClient/PowerPlantManagerClient_ReceiveFSM.h @@ -0,0 +1,88 @@ + + +#ifndef POWERPLANTMANAGERCLIENT_RECEIVEFSM_H +#define POWERPLANTMANAGERCLIENT_RECEIVEFSM_H + +#include "JausUtils.h" +#include "InternalEvents/InternalEventHandler.h" +#include "Transport/JausTransport.h" +#include "JTSStateMachine.h" +#include "urn_jaus_jss_ugv_PowerPlantManagerClient/Messages/MessageSet.h" +#include "urn_jaus_jss_ugv_PowerPlantManagerClient/InternalEvents/InternalEventsSet.h" + +#include "InternalEvents/Receive.h" +#include "InternalEvents/Send.h" + +#include "urn_jaus_jss_core_Transport/Transport_ReceiveFSM.h" +#include "urn_jaus_jss_core_EventsClient/EventsClient_ReceiveFSM.h" +#include "urn_jaus_jss_core_AccessControlClient/AccessControlClient_ReceiveFSM.h" + +#include +#include +#include +#include +#include + +#include "PowerPlantManagerClient_ReceiveFSM_sm.h" + +namespace urn_jaus_jss_ugv_PowerPlantManagerClient +{ + +class DllExport PowerPlantManagerClient_ReceiveFSM : public JTS::StateMachine, public iop::ocu::SlaveHandlerInterface, public iop::EventHandlerInterface +{ +public: + PowerPlantManagerClient_ReceiveFSM(urn_jaus_jss_core_Transport::Transport_ReceiveFSM* pTransport_ReceiveFSM, urn_jaus_jss_core_EventsClient::EventsClient_ReceiveFSM* pEventsClient_ReceiveFSM, urn_jaus_jss_core_AccessControlClient::AccessControlClient_ReceiveFSM* pAccessControlClient_ReceiveFSM); + virtual ~PowerPlantManagerClient_ReceiveFSM(); + + /// Handle notifications on parent state changes + virtual void setupNotifications(); + + /// Action Methods + virtual void handleReportPowerPlantCapabilitiesAction(ReportPowerPlantCapabilities msg, Receive::Body::ReceiveRec transportData); + virtual void handleReportPowerPlantConfigurationAction(ReportPowerPlantConfiguration msg, Receive::Body::ReceiveRec transportData); + virtual void handleReportPowerPlantStatusAction(ReportPowerPlantStatus msg, Receive::Body::ReceiveRec transportData); + + + /// EventHandlerInterface Methods + void event(JausAddress reporter, unsigned short query_msg_id, unsigned int reportlen, const unsigned char* reportdata); + + /// SlaveHandlerInterface Methods + void control_allowed(std::string service_uri, JausAddress component, unsigned char authority); + void enable_monitoring_only(std::string service_uri, JausAddress component); + void access_deactivated(std::string service_uri, JausAddress component); + void create_events(std::string service_uri, JausAddress component, bool by_query=false); + void cancel_events(std::string service_uri, JausAddress component, bool by_query=false); + + /// Guard Methods + + + + PowerPlantManagerClient_ReceiveFSMContext *context; + +protected: + + /// References to parent FSMs + urn_jaus_jss_core_Transport::Transport_ReceiveFSM* pTransport_ReceiveFSM; + urn_jaus_jss_core_EventsClient::EventsClient_ReceiveFSM* pEventsClient_ReceiveFSM; + urn_jaus_jss_core_AccessControlClient::AccessControlClient_ReceiveFSM* pAccessControlClient_ReceiveFSM; + + QueryPowerPlantStatus p_query_states; + JausAddress p_remote_addr; + bool p_has_access; + ros::NodeHandle p_nh; + ros::Timer p_query_timer; + bool p_by_query; + bool p_valid_configuration; + double p_hz; + + bool p_battery_valid; + int p_battery_id; + ros::Publisher p_pub_battery_voltage; + ros::Publisher p_pub_battery_capacity_percent; + void pQueryCallback(const ros::TimerEvent& event); + +}; + +}; + +#endif // PowerPlantManagerCLIENT_RECEIVEFSM_H diff --git a/iop_client_power_plant_fkie/package.xml b/iop_client_power_plant_fkie/package.xml new file mode 100644 index 0000000..e44363e --- /dev/null +++ b/iop_client_power_plant_fkie/package.xml @@ -0,0 +1,24 @@ + + + iop_client_power_plant_fkie + 1.1.0 + + The PowerPlantManager client request battery states from PowerPlantManager services. + + + Alexander Tiderko + Alexander Tiderko + GPLv2 + https://github.com/fkie/iop_jaus_ugv_clients + + catkin + iop_component_fkie + iop_ocu_slavelib_fkie + roscpp + std_msgs + + + + + + diff --git a/iop_client_power_plant_fkie/plugin_iop.xml b/iop_client_power_plant_fkie/plugin_iop.xml new file mode 100644 index 0000000..64959ed --- /dev/null +++ b/iop_client_power_plant_fkie/plugin_iop.xml @@ -0,0 +1,10 @@ + + + + PowerPlantManager client Service Plugin, used e.g. by OCU + + + + + + diff --git a/iop_client_power_plant_fkie/src/PowerPlantManagerClientPlugin.cpp b/iop_client_power_plant_fkie/src/PowerPlantManagerClientPlugin.cpp new file mode 100644 index 0000000..62cbdfd --- /dev/null +++ b/iop_client_power_plant_fkie/src/PowerPlantManagerClientPlugin.cpp @@ -0,0 +1,54 @@ +/** +ROS/IOP Bridge +Copyright (c) 2017 Fraunhofer + +This program is dual licensed; you can redistribute it and/or +modify it under the terms of the GNU General Public License +version 2 as published by the Free Software Foundation, or +enter into a proprietary license agreement with the copyright +holder. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; or you can read the full license at + +*/ + +/** \author Alexander Tiderko */ + +#include +#include "PowerPlantManagerClientPlugin.h" + +using namespace iop; +using namespace urn_jaus_jss_ugv_PowerPlantManagerClient; +using namespace urn_jaus_jss_core_AccessControlClient; +using namespace urn_jaus_jss_core_EventsClient; +using namespace urn_jaus_jss_core_Transport; + + +PowerPlantManagerClientPlugin::PowerPlantManagerClientPlugin() +{ + p_my_service = NULL; + p_base_service = NULL; + p_events_service = NULL; + p_transport_service = NULL; +} + +JTS::Service* PowerPlantManagerClientPlugin::get_service() +{ + return p_my_service; +} + +void PowerPlantManagerClientPlugin::create_service(JTS::JausRouter* jaus_router) +{ + p_base_service = static_cast(get_base_service()); + p_events_service = static_cast(get_base_service(2)); + p_transport_service = static_cast(get_base_service(3)); + p_my_service = new PowerPlantManagerClientService(jaus_router, p_transport_service, p_events_service, p_base_service); +} + +PLUGINLIB_EXPORT_CLASS(iop::PowerPlantManagerClientPlugin, iop::PluginInterface) diff --git a/iop_client_power_plant_fkie/src/PowerPlantManagerClientPlugin.h b/iop_client_power_plant_fkie/src/PowerPlantManagerClientPlugin.h new file mode 100644 index 0000000..042f562 --- /dev/null +++ b/iop_client_power_plant_fkie/src/PowerPlantManagerClientPlugin.h @@ -0,0 +1,54 @@ +/** +ROS/IOP Bridge +Copyright (c) 2017 Fraunhofer + +This program is dual licensed; you can redistribute it and/or +modify it under the terms of the GNU General Public License +version 2 as published by the Free Software Foundation, or +enter into a proprietary license agreement with the copyright +holder. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; or you can read the full license at + +*/ + +/** \author Alexander Tiderko */ + +#ifndef POWERPLANTMANAGERCLIENTPLUGIN_H +#define POWERPLANTMANAGERCLIENTPLUGIN_H + +#include "urn_jaus_jss_ugv_PowerPlantManagerClient/PowerPlantManagerClientService.h" +#include "urn_jaus_jss_core_AccessControlClient/AccessControlClientService.h" +#include "urn_jaus_jss_core_EventsClient/EventsClientService.h" +#include "urn_jaus_jss_core_Transport/TransportService.h" + +#include + +namespace iop +{ + +class DllExport PowerPlantManagerClientPlugin : public PluginInterface +{ +public: + PowerPlantManagerClientPlugin(); + + JTS::Service* get_service(); + void create_service(JTS::JausRouter* jaus_router); + +protected: + urn_jaus_jss_ugv_PowerPlantManagerClient::PowerPlantManagerClientService *p_my_service; + urn_jaus_jss_core_AccessControlClient::AccessControlClientService *p_base_service; + urn_jaus_jss_core_EventsClient::EventsClientService *p_events_service; + urn_jaus_jss_core_Transport::TransportService *p_transport_service; + +}; + +}; + +#endif diff --git a/iop_client_power_plant_fkie/src/main.cpp b/iop_client_power_plant_fkie/src/main.cpp new file mode 100644 index 0000000..4867e1f --- /dev/null +++ b/iop_client_power_plant_fkie/src/main.cpp @@ -0,0 +1 @@ +// it is a library, we don't need this main diff --git a/iop_client_power_plant_fkie/src/urn_jaus_jss_ugv_PowerPlantManagerClient/PowerPlantManagerClient_ReceiveFSM.cpp b/iop_client_power_plant_fkie/src/urn_jaus_jss_ugv_PowerPlantManagerClient/PowerPlantManagerClient_ReceiveFSM.cpp new file mode 100644 index 0000000..4a42e98 --- /dev/null +++ b/iop_client_power_plant_fkie/src/urn_jaus_jss_ugv_PowerPlantManagerClient/PowerPlantManagerClient_ReceiveFSM.cpp @@ -0,0 +1,196 @@ + + +#include "urn_jaus_jss_ugv_PowerPlantManagerClient/PowerPlantManagerClient_ReceiveFSM.h" + +#include +#include + + + +using namespace JTS; +using namespace iop::ocu; + +namespace urn_jaus_jss_ugv_PowerPlantManagerClient +{ + + + +PowerPlantManagerClient_ReceiveFSM::PowerPlantManagerClient_ReceiveFSM(urn_jaus_jss_core_Transport::Transport_ReceiveFSM* pTransport_ReceiveFSM, urn_jaus_jss_core_EventsClient::EventsClient_ReceiveFSM* pEventsClient_ReceiveFSM, urn_jaus_jss_core_AccessControlClient::AccessControlClient_ReceiveFSM* pAccessControlClient_ReceiveFSM) +{ + + /* + * If there are other variables, context must be constructed last so that all + * class variables are available if an EntryAction of the InitialState of the + * statemachine needs them. + */ + context = new PowerPlantManagerClient_ReceiveFSMContext(*this); + + this->pTransport_ReceiveFSM = pTransport_ReceiveFSM; + this->pEventsClient_ReceiveFSM = pEventsClient_ReceiveFSM; + this->pAccessControlClient_ReceiveFSM = pAccessControlClient_ReceiveFSM; + p_has_access = false; + p_by_query = false; + p_valid_configuration = false; + p_hz = 0.0; + p_battery_valid = false; + p_battery_id = 0; +} + + + +PowerPlantManagerClient_ReceiveFSM::~PowerPlantManagerClient_ReceiveFSM() +{ + delete context; +} + +void PowerPlantManagerClient_ReceiveFSM::setupNotifications() +{ + pAccessControlClient_ReceiveFSM->registerNotification("Receiving_Ready", ieHandler, "InternalStateChange_To_PowerPlantManagerClient_ReceiveFSM_Receiving_Ready", "AccessControlClient_ReceiveFSM"); + pAccessControlClient_ReceiveFSM->registerNotification("Receiving", ieHandler, "InternalStateChange_To_PowerPlantManagerClient_ReceiveFSM_Receiving_Ready", "AccessControlClient_ReceiveFSM"); + registerNotification("Receiving_Ready", pAccessControlClient_ReceiveFSM->getHandler(), "InternalStateChange_To_AccessControlClient_ReceiveFSM_Receiving_Ready", "PowerPlantManagerClient_ReceiveFSM"); + registerNotification("Receiving", pAccessControlClient_ReceiveFSM->getHandler(), "InternalStateChange_To_AccessControlClient_ReceiveFSM_Receiving", "PowerPlantManagerClient_ReceiveFSM"); + iop::Config cfg("~PowerPlantManagerClient"); + cfg.param("hz", p_hz, p_hz, false, false); + + // initialize the control layer, which handles the access control staff + Slave &slave = Slave::get_instance(*(jausRouter->getJausAddress())); + slave.add_supported_service(*this, "urn:jaus:jss:ugv:PowerPlantManager", 1, 0); +} + +void PowerPlantManagerClient_ReceiveFSM::handleReportPowerPlantCapabilitiesAction(ReportPowerPlantCapabilities msg, Receive::Body::ReceiveRec transportData) +{ + p_valid_configuration = true; + unsigned int cap_size = msg.getBody()->getPowerPlantCapabilitiesList()->getNumberOfElements(); + for (unsigned int capidx = 0; capidx < cap_size; capidx++) { + ReportPowerPlantCapabilities::body::powerPlantCapabilitiesList::powerPlantCapabilitiesSeq* capsec; + capsec = msg.getBody()->getPowerPlantCapabilitiesList()->getElement(capidx); + if (capsec->getPowerPlantCapabilitiesVar()->getFieldValue() == 2) { + // it is battery + iop::Config cfg("~PowerPlantManagerClient"); + int ppid = capsec->getPowerPlantDescRec()->getPowerPlantID(); + p_battery_id = ppid; + std::stringstream ss; + ss << (int)ppid; + std::string idstr("powerplant_"); + idstr += ss.str(); + p_battery_valid = true; + p_pub_battery_voltage = cfg.advertise(idstr + "/voltage", 5); + p_pub_battery_capacity_percent = cfg.advertise(idstr + "/capacity_percent", 5); + // TODO: implement more then one power plant + break; + } + } + // create event or timer for queries + p_query_timer.stop(); + if (p_remote_addr.get() != 0) { + if (p_by_query) { + if (p_hz > 0) { + ROS_INFO_NAMED("PowerPlantManagerClient", "create QUERY timer to get power plant states from %s", p_remote_addr.str().c_str()); + p_query_timer = p_nh.createTimer(ros::Duration(1.0 / p_hz), &PowerPlantManagerClient_ReceiveFSM::pQueryCallback, this); + } else { + ROS_WARN_NAMED("PowerPlantManagerClient", "invalid hz %.2f for QUERY timer to get power plant states from %s", p_hz, p_remote_addr.str().c_str()); + } + } else { + ROS_INFO_NAMED("PowerPlantManagerClient", "create EVENT to get power plant states from %s", p_remote_addr.str().c_str()); + pEventsClient_ReceiveFSM->create_event(*this, p_remote_addr, p_query_states, p_hz); + } + } +} + +void PowerPlantManagerClient_ReceiveFSM::handleReportPowerPlantConfigurationAction(ReportPowerPlantConfiguration msg, Receive::Body::ReceiveRec transportData) +{ + ROS_DEBUG_NAMED("PowerPlantManagerClient", "handle ReportPowerPlantConfiguration from %s not implemented", p_remote_addr.str().c_str()); +} + +void PowerPlantManagerClient_ReceiveFSM::handleReportPowerPlantStatusAction(ReportPowerPlantStatus msg, Receive::Body::ReceiveRec transportData) +{ + if (p_battery_valid) { + unsigned int cap_size = msg.getBody()->getPowerPlantStatusList()->getNumberOfElements(); + for (unsigned int capidx = 0; capidx < cap_size; capidx++) { + ReportPowerPlantStatus::body::powerPlantStatusList::powerPlantStatus* capsec; + capsec = msg.getBody()->getPowerPlantStatusList()->getElement(capidx); + if (capsec->getPowerPlantDescRec()->getPowerPlantID() == p_battery_id) { + if (capsec->getPowerPlantStatusVar()->getFieldValue() == 2) { + unsigned int bat_count = capsec->getPowerPlantStatusVar()->getBatteryStatus()->getNumberOfElements(); + for (unsigned int batidx = 0; batidx < bat_count; batidx++) { + ReportPowerPlantStatus::body::powerPlantStatusList::powerPlantStatus::powerPlantStatusVar::batteryStatus::batteryStatusRec* batrec; + batrec = capsec->getPowerPlantStatusVar()->getBatteryStatus()->getElement(batidx); + std_msgs::Float32 ros_msg_volt; + ros_msg_volt.data = batrec->getVoltage(); + p_pub_battery_voltage.publish(ros_msg_volt); + std_msgs::Int8 ros_msg_pc; + ros_msg_pc.data = batrec->getPercentChargeRemaining(); + p_pub_battery_capacity_percent.publish(ros_msg_pc); + // TODO: implement more then one power plant + break; + } + } + break; + } + } + } +} + +void PowerPlantManagerClient_ReceiveFSM::control_allowed(std::string service_uri, JausAddress component, unsigned char authority) +{ + if (service_uri.compare("urn:jaus:jss:ugv:PowerPlantManager") == 0) { + p_remote_addr = component; + p_has_access = true; + } else { + ROS_WARN_STREAM("[PowerPlantManagerClient] unexpected control allowed for " << service_uri << " received, ignored!"); + } +} + +void PowerPlantManagerClient_ReceiveFSM::enable_monitoring_only(std::string service_uri, JausAddress component) +{ + p_remote_addr = component; +} + +void PowerPlantManagerClient_ReceiveFSM::access_deactivated(std::string service_uri, JausAddress component) +{ + p_has_access = false; + p_remote_addr = JausAddress(0); +} + +void PowerPlantManagerClient_ReceiveFSM::create_events(std::string service_uri, JausAddress component, bool by_query) +{ + p_by_query = by_query; + p_query_timer = p_nh.createTimer(ros::Duration(1), &PowerPlantManagerClient_ReceiveFSM::pQueryCallback, this); +} + +void PowerPlantManagerClient_ReceiveFSM::cancel_events(std::string service_uri, JausAddress component, bool by_query) +{ + p_query_timer.stop(); + if (!by_query) { + ROS_INFO_NAMED("IlluminationClient", "cancel EVENT for power plant state by %s", component.str().c_str()); + pEventsClient_ReceiveFSM->cancel_event(*this, component, p_query_states); + } + p_valid_configuration = false; + p_battery_valid = false; +} + +void PowerPlantManagerClient_ReceiveFSM::pQueryCallback(const ros::TimerEvent& event) +{ + if (p_remote_addr.get() != 0) { + if (!p_valid_configuration) { + QueryPowerPlantCapabilities query_cap_msg; + sendJausMessage(query_cap_msg, p_remote_addr); + } else { + sendJausMessage(p_query_states, p_remote_addr); + } + } +} + +void PowerPlantManagerClient_ReceiveFSM::event(JausAddress sender, unsigned short query_msg_id, unsigned int reportlen, const unsigned char* reportdata) +{ + ReportPowerPlantStatus report; + report.decode(reportdata); + Receive::Body::ReceiveRec transport_data; + transport_data.setSrcSubsystemID(sender.getSubsystemID()); + transport_data.setSrcNodeID(sender.getNodeID()); + transport_data.setSrcComponentID(sender.getComponentID()); + handleReportPowerPlantStatusAction(report, transport_data); +} + + +};