added local waypoint list driver service

iop_local_waypoint_list_driver_fkie/CMakeLists.txt

@@ -0,0 +1,89 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(iop_local_waypoint_list_driver_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_list_manager_fkie
+             nav_msgs
+             roscpp
+             std_msgs
+             tf
+)
+iop_init(COMPONENT_ID 0)
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package(
+    LIBRARIES ${PROJECT_NAME}
+    CATKIN_DEPENDS
+        iop_component_fkie
+        iop_list_manager_fkie
+        nav_msgs
+        std_msgs
+        tf
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+iop_code_generator(
+  IDLS
+    urn.jaus.jss.core-v1.1/AccessControl.xml
+    urn.jaus.jss.core-v1.1/Events.xml
+    urn.jaus.jss.core-v1.1/Management.xml
+    urn.jaus.jss.core-v1.1/Transport.xml
+    urn.jaus.jss.core-v1.1/ListManager.xml
+    urn.jaus.jss.mobility/LocalWaypointListDriver.xml
+  OWN_IDLS
+  OVERRIDES
+    include/urn_jaus_jss_mobility_LocalWaypointListDriver/LocalWaypointListDriver_ReceiveFSM.h
+    src/urn_jaus_jss_mobility_LocalWaypointListDriver/LocalWaypointListDriver_ReceiveFSM.cpp
+    src/main.cpp
+  EXTERN_SERVICES
+    urn_jaus_jss_core_AccessControl
+    urn_jaus_jss_core_Events
+    urn_jaus_jss_core_ListManager
+    urn_jaus_jss_core_Management
+    urn_jaus_jss_core_Transport
+  GENERATED_SOURCES cppfiles
+)
+
+include_directories(${catkin_INCLUDE_DIRS})
+
+add_library(${PROJECT_NAME}
+            src/LocalWaypointListDriverPlugin.cpp
+            ${cppfiles}
+)
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}
+                      ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+install(
+  DIRECTORY ${IOP_INSTALL_INCLUDE_DIRS} 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}
+)

iop_local_waypoint_list_driver_fkie/README.md

@@ -0,0 +1,33 @@
+This package is part of [ROS/IOP Bridge](https://github.com/fkie/iop_core/blob/master/README.md).
+
+
+## _iop_local_waypoint_list_driver_fkie:_ LocalWaypointListDriver
+
+Forwards the list of local waypoints to ROS.
+
+#### Parameter:
+
+_tf_frame_robot (str_, Default: "base_link")
+
+> TF frame used in ROS for local coordinates. This value is set in each command message.
+
+_tv_max (float_ , Default: 1.0)
+
+> The maximum allowed speed.
+
+#### Publisher:
+
+_cmd_local_waypoints (nav_msgs::Path)_
+
+> The list with local waypoints. It can also be an empty list to abort the execution.
+
+_cmd_travel_speed (std_msgs::Float32)_, latched
+
+> The maximum speed configured by parameter or send from OCU. Speed from OCU is always smaller or equal to parameterized value.
+
+#### Subscriber:
+
+_local_way_points_finished (std_msgs::Bool)_
+
+> Reports to the client service that the excution is finished.
+

iop_local_waypoint_list_driver_fkie/include/urn_jaus_jss_mobility_LocalWaypointListDriver/LocalWaypointListDriver_ReceiveFSM.h

@@ -0,0 +1,90 @@
+
+
+#ifndef LOCALWAYPOINTLISTDRIVER_RECEIVEFSM_H
+#define LOCALWAYPOINTLISTDRIVER_RECEIVEFSM_H
+
+#include "JausUtils.h"
+#include "InternalEvents/InternalEventHandler.h"
+#include "Transport/JausTransport.h"
+#include "JTSStateMachine.h"
+#include "urn_jaus_jss_mobility_LocalWaypointListDriver/Messages/MessageSet.h"
+#include "urn_jaus_jss_mobility_LocalWaypointListDriver/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_Events/Events_ReceiveFSM.h"
+#include "urn_jaus_jss_core_AccessControl/AccessControl_ReceiveFSM.h"
+#include "urn_jaus_jss_core_Management/Management_ReceiveFSM.h"
+#include "urn_jaus_jss_core_ListManager/ListManager_ReceiveFSM.h"
+
+#include <ros/ros.h>
+#include <nav_msgs/Path.h>
+#include <std_msgs/Float32.h>
+#include <std_msgs/Bool.h>
+
+#include <iop_list_manager_fkie/ListManagerListenerInterface.h>
+
+#include "LocalWaypointListDriver_ReceiveFSM_sm.h"
+
+namespace urn_jaus_jss_mobility_LocalWaypointListDriver
+{
+
+class DllExport LocalWaypointListDriver_ReceiveFSM : public JTS::StateMachine, public iop::ListManagerListenerInterface
+{
+public:
+	LocalWaypointListDriver_ReceiveFSM(urn_jaus_jss_core_Transport::Transport_ReceiveFSM* pTransport_ReceiveFSM, urn_jaus_jss_core_Events::Events_ReceiveFSM* pEvents_ReceiveFSM, urn_jaus_jss_core_AccessControl::AccessControl_ReceiveFSM* pAccessControl_ReceiveFSM, urn_jaus_jss_core_Management::Management_ReceiveFSM* pManagement_ReceiveFSM, urn_jaus_jss_core_ListManager::ListManager_ReceiveFSM* pListManager_ReceiveFSM);
+	virtual ~LocalWaypointListDriver_ReceiveFSM();
+
+	/// Handle notifications on parent state changes
+	virtual void setupNotifications();
+
+	/// Action Methods
+	virtual void executeWaypointListAction(ExecuteList msg, Receive::Body::ReceiveRec transportData);
+	virtual void modifyTravelSpeedAction(ExecuteList msg);
+	virtual void resetTravelSpeedAction();
+	virtual void sendReportActiveElementAction(QueryActiveElement msg, Receive::Body::ReceiveRec transportData);
+	virtual void sendReportLocalWaypointAction(QueryLocalWaypoint msg, Receive::Body::ReceiveRec transportData);
+	virtual void sendReportTravelSpeedAction(QueryTravelSpeed msg, Receive::Body::ReceiveRec transportData);
+
+
+	/// Guard Methods
+	virtual bool elementExists(ExecuteList msg);
+	virtual bool elementSpecified(ExecuteList msg);
+	virtual bool isControllingClient(Receive::Body::ReceiveRec transportData);
+
+	/// ListManagerListenerInterface
+	void execute_list(std::vector<iop::InternalElement> elements, double speed = std::numeric_limits<double>::quiet_NaN());
+	void stop_execution();
+
+	LocalWaypointListDriver_ReceiveFSMContext *context;
+
+protected:
+
+    /// References to parent FSMs
+	urn_jaus_jss_core_Transport::Transport_ReceiveFSM* pTransport_ReceiveFSM;
+	urn_jaus_jss_core_Events::Events_ReceiveFSM* pEvents_ReceiveFSM;
+	urn_jaus_jss_core_AccessControl::AccessControl_ReceiveFSM* pAccessControl_ReceiveFSM;
+	urn_jaus_jss_core_Management::Management_ReceiveFSM* pManagement_ReceiveFSM;
+	urn_jaus_jss_core_ListManager::ListManager_ReceiveFSM* pListManager_ReceiveFSM;
+
+	ros::Publisher p_pub_path;
+	ros::Publisher p_pub_tv_max;
+	ros::Subscriber p_sub_finished;
+	float p_travel_speed;
+	float p_tv_max;
+	std::string p_tf_frame_robot;
+	ReportLocalWaypoint p_current_waypoint;
+	ReportActiveElement p_current_element;
+	bool p_executing;
+	jUnsignedShortInteger p_last_uid;
+
+	void pRosFinished(const std_msgs::Bool::ConstPtr& state);
+	geometry_msgs::PoseStamped get_pose_from_waypoint(iop::InternalElement& element, bool update_current=false);
+
+};
+
+};
+
+#endif // GLOBALWAYPOINTLISTDRIVER_RECEIVEFSM_H

iop_local_waypoint_list_driver_fkie/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>iop_local_waypoint_list_driver_fkie</name>
+  <version>1.0.0</version>
+  <description>Service to move the platform to specified list of waypoint.</description>
+
+  <author>Alexander Tiderko</author>
+  <maintainer email="alexander.tiderko@fkie.fraunhofer.de">Alexander Tiderko</maintainer>
+  <license>GPLv2</license>
+  <url type="website">https://github.com/fkie/iop_jaus_mobility</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>iop_component_fkie</depend>
+  <depend>iop_list_manager_fkie</depend>
+  <depend>nav_msgs</depend>
+  <depend>roscpp</depend>
+  <depend>std_msgs</depend>
+  <depend>tf</depend>
+
+  <export>
+    <iop_component_fkie plugin="${prefix}/plugin_iop.xml" />
+  </export>
+
+</package>

iop_local_waypoint_list_driver_fkie/plugin_iop.xml

@@ -0,0 +1,10 @@
+<library path="libiop_local_waypoint_list_driver_fkie">
+  <class name="LocalWaypointListDriver" type="iop::LocalWaypointListDriverPlugin" base_class_type="iop::PluginInterface">
+    <description>
+      LocalWaypointListDriver Service Plugin
+    </description>
+    <iop_service name="LocalWaypointListDriver" id="urn:jaus:jss:mobility:LocalWaypointListDriver" version="1.0">
+      <inherits_from id="urn:jaus:jss:core:ListManager" min_version="1.1"/>
+    </iop_service>
+  </class>
+</library>

iop_local_waypoint_list_driver_fkie/src/LocalWaypointListDriverPlugin.cpp

@@ -0,0 +1,60 @@
+/**
+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
+<http://www.gnu.de/documents/gpl-2.0.html>
+*/
+
+/** \author Alexander Tiderko */
+
+#include <pluginlib/class_list_macros.h>
+#include "LocalWaypointListDriverPlugin.h"
+
+using namespace iop;
+using namespace urn_jaus_jss_mobility_LocalWaypointListDriver;
+using namespace urn_jaus_jss_core_ListManager;
+using namespace urn_jaus_jss_core_Management;
+using namespace urn_jaus_jss_core_AccessControl;
+using namespace urn_jaus_jss_core_Events;
+using namespace urn_jaus_jss_core_Transport;
+
+
+LocalWaypointListDriverPlugin::LocalWaypointListDriverPlugin()
+{
+	p_my_service = NULL;
+	p_base_service = NULL;
+	p_mgmt_service = NULL;
+	p_accesscontrol_service = NULL;
+	p_events_service = NULL;
+	p_transport_service = NULL;
+}
+
+JTS::Service* LocalWaypointListDriverPlugin::get_service()
+{
+	return p_my_service;
+}
+
+void LocalWaypointListDriverPlugin::create_service(JTS::JausRouter* jaus_router)
+{
+	p_base_service = static_cast<ListManagerService *>(get_base_service());
+	p_mgmt_service = static_cast<ManagementService *>(get_base_service(2));
+	p_accesscontrol_service = static_cast<AccessControlService *>(get_base_service(3));
+	p_events_service = static_cast<EventsService *>(get_base_service(4));
+	p_transport_service = static_cast<TransportService *>(get_base_service(5));
+	p_my_service = new LocalWaypointListDriverService(jaus_router, p_transport_service, p_events_service, p_accesscontrol_service, p_mgmt_service, p_base_service);
+}
+
+PLUGINLIB_EXPORT_CLASS(iop::LocalWaypointListDriverPlugin, iop::PluginInterface)

iop_local_waypoint_list_driver_fkie/src/LocalWaypointListDriverPlugin.h

@@ -0,0 +1,59 @@
+/**
+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
+<http://www.gnu.de/documents/gpl-2.0.html>
+*/
+
+/** \author Alexander Tiderko */
+
+
+#ifndef LOCALWAYPOINTLISTDRIVERPLUGIN_H
+#define LOCALWAYPOINTLISTDRIVERPLUGIN_H
+
+#include "urn_jaus_jss_mobility_LocalWaypointListDriver/LocalWaypointListDriverService.h"
+#include "urn_jaus_jss_core_AccessControl/AccessControlService.h"
+#include "urn_jaus_jss_core_Events/EventsService.h"
+#include "urn_jaus_jss_core_ListManager/ListManagerService.h"
+#include "urn_jaus_jss_core_Management/ManagementService.h"
+#include "urn_jaus_jss_core_Transport/TransportService.h"
+
+#include <iop_component_fkie/iop_plugin_interface.h>
+
+namespace iop
+{
+
+class DllExport LocalWaypointListDriverPlugin : public PluginInterface
+{
+public:
+	LocalWaypointListDriverPlugin();
+
+	JTS::Service* get_service();
+	void create_service(JTS::JausRouter* jaus_router);
+
+protected:
+	urn_jaus_jss_mobility_LocalWaypointListDriver::LocalWaypointListDriverService *p_my_service;
+	urn_jaus_jss_core_ListManager::ListManagerService *p_base_service;
+	urn_jaus_jss_core_Management::ManagementService *p_mgmt_service;
+	urn_jaus_jss_core_AccessControl::AccessControlService *p_accesscontrol_service;
+	urn_jaus_jss_core_Events::EventsService *p_events_service;
+	urn_jaus_jss_core_Transport::TransportService *p_transport_service;
+
+};
+
+};
+
+#endif

iop_local_waypoint_list_driver_fkie/src/main.cpp

@@ -0,0 +1 @@
+// it is a library, we don't need this main