visual sensor client can now control power and zoom of visual sensor

iop_client_visual_sensor_fkie/CMakeLists.txt

@@ -25,6 +25,7 @@ iop_export_service(urn_jaus_jss_environmentSensing_VisualSensorClient)
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
+    INCLUDE_DIRS include/public
     LIBRARIES ${PROJECT_NAME}
     CATKIN_DEPENDS
         iop_component_fkie
@@ -59,10 +60,13 @@ iop_code_generator(
   GENERATED_SOURCES cppfiles
 )
 
-include_directories(${catkin_INCLUDE_DIRS})
+include_directories(${catkin_INCLUDE_DIRS}
+                    include/public
+)
 
 ## Declare a cpp executable
 add_library(${PROJECT_NAME}
+            src/VisualSensorClient.cpp
             src/VisualSensorClientPlugin.cpp
             ${cppfiles}
 )
@@ -72,6 +76,19 @@ 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
+)
+
+# Mark executables and/or libraries for installation
 install(TARGETS ${PROJECT_NAME}
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

iop_client_visual_sensor_fkie/README.md

@@ -7,14 +7,41 @@ Gets names for sensor ID's from [VisualSensor](https://github.com/fkie/iop_jaus_
 
 #### Parameter:
 
-> None
+_hz (float_ , Default: 0.0)
+
+> Sets how often configuration 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:
 
 _visual_sensor_names (iop_msgs_fkie::VisualSensorNames)_
 
 > Publishes the discovered pairs of ID and name.
 
+_`ID`/pwr_state (std_msgs::Bool)_
+
+> Publishes the current power state of the sensor. Only available if remote sensor supports states.
+
+_`ID`/zoom_level (std_msgs::Bool)_
+
+> Publishes the current zoom level for the sensor. Only available if remote sensor supports zoom modes.
+
+_`ID`/fov_horizontal (std_msgs::Float32)_
+
+> Publishes the current horizontal field of view for the sensor.
+
+_`ID`/fov_vertical (std_msgs::Float32)_
+
+> Publishes the current vertical field of view for the sensor.
+
+
 #### Subscriber:
 
-> None
+_`ID`/cmd_pwr_state (std_msgs::Bool)_
+
+> Sets the new power state for the sensor. Only available if remote sensor supports states.
+
+_`ID`/cmd_zoom_level (std_msgs::Bool)_
+
+> Sets the new zoom level for the sensor. Only available if remote sensor supports zoom modes.
+

iop_client_visual_sensor_fkie/include/public/iop_client_visual_sensor_fkie/VisualSensorClient.h

@@ -0,0 +1,113 @@
+/**
+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 IOPVISUALSENSORCLIENT_H
+#define IOPVISUALSENSORCLIENT_H
+
+#include "Transport/JausTransport.h"
+#include "urn_jaus_jss_environmentSensing_VisualSensorClient/Messages/MessageSet.h"
+#include "urn_jaus_jss_environmentSensing_VisualSensorClient/InternalEvents/InternalEventsSet.h"
+
+#include <boost/bind.hpp>
+#include <boost/function.hpp>
+#include <ros/ros.h>
+#include <geometry_msgs/Pose.h>
+#include <std_msgs/Bool.h>
+#include <std_msgs/Float32.h>
+#include <string>
+
+typedef urn_jaus_jss_environmentSensing_VisualSensorClient::ReportVisualSensorCapabilities::Body::VisualSensorCapabilitiesList::VisualSensorCapabilitiesRec CapabilityRec;
+typedef urn_jaus_jss_environmentSensing_VisualSensorClient::ReportVisualSensorConfiguration::Body::VisualSensorConfigurationList::VisualSensorConfigurationRec ConfigurationRec;
+typedef urn_jaus_jss_environmentSensing_VisualSensorClient::ReportSensorGeometricProperties::Body::GeometricPropertiesList::GeometricPropertiesSequence GeometricSeq;
+typedef urn_jaus_jss_environmentSensing_VisualSensorClient::SetVisualSensorConfiguration::Body::VisualSensorConfigurationSequence::VisualSensorConfigurationList::VisualSensorConfigurationRec SetConfigurationRec;
+
+namespace iop
+{
+
+class VisualSensorClient {
+
+public:
+	VisualSensorClient(jUnsignedShortInteger id=0);
+	VisualSensorClient(CapabilityRec cap);
+	~VisualSensorClient();
+	bool operator==(VisualSensorClient &value);
+	bool operator!=(VisualSensorClient &value);
+
+	jUnsignedByte get_id() { return p_id; }
+	std::string get_name() { return p_name; }
+	jUnsignedShortInteger get_zoom_level() { return p_zoom_level; }
+	bool get_switch_state() { return p_switch_state; } // 0: Active, 1: Standby, 2: Off
+	JausAddress get_manipulator() { return p_manipulator; }
+	jUnsignedByte get_joint_nr() { return p_joint_nr; }
+	geometry_msgs::Pose get_pose() { return p_pose; }
+
+	/** Returns true if it was initialized and has valid id */
+	bool is_valid() { return p_id != 0 && p_id != 65535; }
+	bool is_zoomable() { return p_zoomable; }
+	bool is_switchable() { return p_switchable; }
+	bool is_manipulator_valid() { return p_manipulator.get() != 0; }
+	bool is_joint_valid() { return p_joint_nr != std::numeric_limits<uint8_t>::max(); }
+	bool is_pose_valid() { return p_pose_valid; }
+
+	void apply_capability(CapabilityRec cap);
+	void apply_configuration(ConfigurationRec conf);
+	void apply_geometric(GeometricSeq geo);
+	SetConfigurationRec get_configuration();
+
+	template<class T>
+	void set_state_callback(void(T::*handler)(jUnsignedShortInteger id, SetConfigurationRec cfg), T*obj) {
+		p_state_callback = boost::bind(handler, obj, _1, _2);
+	}
+
+protected:
+	boost::function<void (jUnsignedShortInteger id, SetConfigurationRec cfg)> p_state_callback;
+	jUnsignedShortInteger p_id;
+	std::string p_name;
+	bool p_switchable;  // 0: Active, 1: Standby, 2: Off
+	bool p_switch_state;
+	bool p_zoomable;  // 0: MixedZoom, 1: AnalogZoomOnly, 2: DigitalZoomOnly, 3: NoZoom
+	jUnsignedShortInteger p_zoom_level;
+	JausAddress p_manipulator;
+	jUnsignedByte p_joint_nr;
+	geometry_msgs::Pose p_pose;
+	bool p_pose_valid;
+	double p_fov_horizontal;
+	double p_fov_vertical;
+	ros::Publisher p_pub_state;
+	ros::Subscriber p_sub_state;
+	ros::Publisher p_pub_zoom_level;
+	ros::Subscriber p_sub_zoom_level;
+	ros::Publisher p_pub_fov_horizontal;
+	ros::Publisher p_pub_fov_vertical;
+
+private:
+	VisualSensorClient(VisualSensorClient const& from);
+	const VisualSensorClient& operator=(const VisualSensorClient& from);
+
+	void p_ros_state(const std_msgs::Bool::ConstPtr& state);
+	void p_ros_zoom_level(const std_msgs::Float32::ConstPtr& state);
+};
+
+};
+
+#endif

iop_client_visual_sensor_fkie/include/urn_jaus_jss_environmentSensing_VisualSensorClient/VisualSensorClient_ReceiveFSM.h

@@ -20,15 +20,17 @@
 
 #include "VisualSensorClient_ReceiveFSM_sm.h"
 
+#include <boost/thread/recursive_mutex.hpp>
 #include <ros/ros.h>
 #include <std_msgs/UInt16.h>
 #include <iop_ocu_slavelib_fkie/SlaveHandlerInterface.h>
-
+#include <iop_events_fkie/EventHandlerInterface.h>
+#include <iop_client_visual_sensor_fkie/VisualSensorClient.h>
 
 namespace urn_jaus_jss_environmentSensing_VisualSensorClient
 {
 
-class DllExport VisualSensorClient_ReceiveFSM : public JTS::StateMachine, public iop::ocu::SlaveHandlerInterface
+class DllExport VisualSensorClient_ReceiveFSM : public JTS::StateMachine, public iop::ocu::SlaveHandlerInterface, public iop::EventHandlerInterface
 {
 public:
 	VisualSensorClient_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);
@@ -52,6 +54,8 @@ class DllExport VisualSensorClient_ReceiveFSM : public JTS::StateMachine, public
 	void cancel_events(std::string service_uri, JausAddress component, bool by_query=false);
 	/// Guard Methods
 
+	/// EventHandlerInterface Methods
+	void event(JausAddress reporter, unsigned short query_msg_id, unsigned int reportlen, const unsigned char* reportdata);
 	std::string get_sensor_name(JausAddress &component, unsigned short id);
 
 
@@ -64,15 +68,27 @@ class DllExport VisualSensorClient_ReceiveFSM : public JTS::StateMachine, public
 	urn_jaus_jss_core_EventsClient::EventsClient_ReceiveFSM* pEventsClient_ReceiveFSM;
 	urn_jaus_jss_core_AccessControlClient::AccessControlClient_ReceiveFSM* pAccessControlClient_ReceiveFSM;
 
+	typedef boost::recursive_mutex mutex_type;
+	typedef boost::unique_lock<mutex_type> lock_type;
+	mutable mutex_type p_mutex;
+
 	JausAddress p_remote_addr;
 	ros::NodeHandle p_nh;
 	bool p_has_access;
+	int p_query_state;
+	bool p_by_query;
+	double p_hz;
+	unsigned char p_request_id;
 	ros::Timer p_query_timer;
 	ros::Publisher p_pub_visual_sensor_names;
 	QueryVisualSensorCapabilities p_query_caps;
+	QuerySensorGeometricProperties p_query_geo;
+	QueryVisualSensorConfiguration p_query_cfgs;
 	std::map<unsigned int, std::map<unsigned short, std::string> > p_sensor_names;  // JausAddress, sensor ID, name
+	std::map<jUnsignedShortInteger, boost::shared_ptr<iop::VisualSensorClient> > p_sensors;
 
 	void pQueryCallback(const ros::TimerEvent& event);
+	void p_state_changed(jUnsignedShortInteger id, SetConfigurationRec cfg);
 
 };