add fading/ resetting leds services

CMakeLists.txt

@@ -48,6 +48,8 @@ set(
   src/services/robot_config.cpp
   src/services/set_language.cpp
   src/services/get_language.cpp
+  src/services/fade_leds.cpp
+  src/services/reset_leds.cpp
   )
 
 set(

src/helpers/driver_helpers.cpp

@@ -223,6 +223,24 @@ std::string& getLanguage( const qi::SessionPtr& session )
   return language;
 }
 
+/** Function that fades eye leds
+ */
+std_msgs::Empty& fadeLeds( const qi::SessionPtr& session, naoqi_bridge_msgs::FadeLedsRequest req )
+{
+  std::cout << "Receiving service call of fading leds" << std::endl;
+  qi::AnyObject p_leds = session->service("ALLeds");
+  p_leds.call<void>("fadeRGB", req.fade_rgb.led_name, req.fade_rgb.color.r, req.fade_rgb.color.g, req.fade_rgb.color.b, req.fade_rgb.fade_duration.toSec());
+}
+
+/** Function that reset leds color
+ */
+std_msgs::Empty& resetLeds( const qi::SessionPtr& session, naoqi_bridge_msgs::SetStringRequest req )
+{
+  std::cout << "Receiving service call of resetting leds" << std::endl;
+  qi::AnyObject p_leds = session->service("ALLeds");
+  p_leds.call<void>("reset", req.data);
+}
+
 } // driver
 } // helpers
 } // naoqi

src/helpers/driver_helpers.hpp

@@ -23,8 +23,12 @@
 
 #include <naoqi_bridge_msgs/RobotInfo.h>
 
+#include <std_msgs/Empty.h>
+
 #include <naoqi_bridge_msgs/SetString.h>
 
+#include <naoqi_bridge_msgs/FadeLeds.h>
+
 #include <qi/applicationsession.hpp>
 
 namespace naoqi
@@ -42,6 +46,10 @@ bool& setLanguage( const qi::SessionPtr& session, naoqi_bridge_msgs::SetStringRe
 
 std::string& getLanguage( const qi::SessionPtr& session );
 
+std_msgs::Empty& fadeLeds( const qi::SessionPtr& session, naoqi_bridge_msgs::FadeLedsRequest req );
+
+std_msgs::Empty& resetLeds( const qi::SessionPtr& session, naoqi_bridge_msgs::SetStringRequest req );
+
 } // driver
 } // helpers
 } // naoqi

src/naoqi_driver.cpp

@@ -71,6 +71,8 @@
 #include "services/robot_config.hpp"
 #include "services/set_language.hpp"
 #include "services/get_language.hpp"
+#include "services/fade_leds.hpp"
+#include "services/reset_leds.hpp"
 
 /*
  * RECORDERS
@@ -893,6 +895,8 @@ void Driver::registerDefaultServices()
   registerService( boost::make_shared<service::RobotConfigService>("robot config service", "/naoqi_driver/get_robot_config", sessionPtr_) );
   registerService( boost::make_shared<service::SetLanguageService>("set language service", "/naoqi_driver/set_language", sessionPtr_) );
   registerService( boost::make_shared<service::GetLanguageService>("get language service", "/naoqi_driver/get_language", sessionPtr_) );
+  registerService( boost::make_shared<service::FadeLedsService>("fade leds service", "/naoqi_driver/fade_leds", sessionPtr_) );
+  registerService( boost::make_shared<service::ResetLedsService>("reset leds service", "/naoqi_driver/reset_leds", sessionPtr_) );
 }
 
 std::vector<std::string> Driver::getAvailableConverters()

src/services/fade_leds.cpp

@@ -0,0 +1,45 @@
+/*
+ * Copyright 2018 Aldebaran
+ *
+ * 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 "fade_leds.hpp"
+#include "../helpers/driver_helpers.hpp"
+
+namespace naoqi
+{
+namespace service
+{
+
+  FadeLedsService::FadeLedsService( const std::string& name, const std::string& topic, const qi::SessionPtr& session )
+    : name_(name),
+      topic_(topic),
+      session_(session)
+  {}
+
+  void FadeLedsService::reset( ros::NodeHandle& nh )
+  {
+    service_ = nh.advertiseService(topic_, &FadeLedsService::callback, this);
+  }
+
+  bool FadeLedsService::callback( naoqi_bridge_msgs::FadeLedsRequest& req, naoqi_bridge_msgs::FadeLedsResponse& resp )
+  {
+    helpers::driver::fadeLeds(session_, req);
+    return true;
+  }
+
+
+}
+}

src/services/fade_leds.hpp

@@ -0,0 +1,67 @@
+/*
+ * Copyright 2018 Aldebaran
+ *
+ * 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.
+ *
+ */
+
+
+#ifndef FADE_LEDS_SERVICE_HPP
+#define FADE_LEDS_SERVICE_HPP
+
+#include <iostream>
+
+#include <ros/node_handle.h>
+#include <ros/service_server.h>
+
+#include <naoqi_bridge_msgs/FadeLeds.h>
+#include <qi/session.hpp>
+
+namespace naoqi
+{
+namespace service
+{
+
+class FadeLedsService
+{
+public:
+  FadeLedsService( const std::string& name, const std::string& topic, const qi::SessionPtr& session );
+
+  ~FadeLedsService(){};
+
+  std::string name() const
+  {
+    return name_;
+  }
+
+  std::string topic() const
+  {
+    return topic_;
+  }
+
+  void reset( ros::NodeHandle& nh );
+
+  bool callback( naoqi_bridge_msgs::FadeLedsRequest& req, naoqi_bridge_msgs::FadeLedsResponse& resp );
+
+
+private:
+  const std::string name_;
+  const std::string topic_;
+
+  const qi::SessionPtr& session_;
+  ros::ServiceServer service_;
+};
+
+} // service
+} // naoqi
+#endif

src/services/reset_leds.cpp

@@ -0,0 +1,46 @@
+/*
+ * Copyright 2018 Aldebaran
+ *
+ * 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 "reset_leds.hpp"
+#include "../helpers/driver_helpers.hpp"
+
+namespace naoqi
+{
+namespace service
+{
+
+  ResetLedsService::ResetLedsService( const std::string& name, const std::string& topic, const qi::SessionPtr& session )
+    : name_(name),
+      topic_(topic),
+      session_(session)
+  {}
+
+  void ResetLedsService::reset( ros::NodeHandle& nh )
+  {
+    service_ = nh.advertiseService(topic_, &ResetLedsService::callback, this);
+  }
+
+  bool ResetLedsService::callback( naoqi_bridge_msgs::SetStringRequest& req, naoqi_bridge_msgs::SetStringResponse& resp )
+  {
+    helpers::driver::resetLeds(session_, req);
+    resp.success = true;
+    return true;
+  }
+
+
+}
+}

src/services/reset_leds.hpp

@@ -0,0 +1,67 @@
+/*
+ * Copyright 2018 Aldebaran
+ *
+ * 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.
+ *
+ */
+
+
+#ifndef RESET_LEDS_SERVICE_HPP
+#define RESET_LEDS_SERVICE_HPP
+
+#include <iostream>
+
+#include <ros/node_handle.h>
+#include <ros/service_server.h>
+
+#include <naoqi_bridge_msgs/SetString.h>
+#include <qi/session.hpp>
+
+namespace naoqi
+{
+namespace service
+{
+
+class ResetLedsService
+{
+public:
+  ResetLedsService( const std::string& name, const std::string& topic, const qi::SessionPtr& session );
+
+  ~ResetLedsService(){};
+
+  std::string name() const
+  {
+    return name_;
+  }
+
+  std::string topic() const
+  {
+    return topic_;
+  }
+
+  void reset( ros::NodeHandle& nh );
+
+  bool callback( naoqi_bridge_msgs::SetStringRequest& req, naoqi_bridge_msgs::SetStringResponse& resp );
+
+
+private:
+  const std::string name_;
+  const std::string topic_;
+
+  const qi::SessionPtr& session_;
+  ros::ServiceServer service_;
+};
+
+} // service
+} // naoqi
+#endif