[control_over] Add Method SetPose (current status: not working)

include/gz_gep_tools/control_over_gz.hh

@@ -6,6 +6,9 @@
 
 /// GZ includes
 #include <gz/msgs.hh>
+#include <gz/msgs/clock.pb.h>
+#include <gz/msgs/world_control.pb.h>
+#include <gz/msgs/world_control_state.pb.h>
 #include <gz/transport.hh>
 
 #include "joint_state_interface.hh"
@@ -36,6 +39,10 @@ class ControlOverGz {
   /// Send World Control state to Init ECM.
   bool SendWorldControlStateToInitECM(const RobotCtrlJointInfos &rbt_ctrl_joint_infos);
 
+  /// Set Pose
+  bool SetPose(double x, double y, double z,
+               double qx, double qy, double qz, double qw);
+
   /// Initialization of the ECM by reading the world control state
   bool ReadWorldStateToInitECM();
 
@@ -44,7 +51,7 @@ class ControlOverGz {
 
   /// Display link values
   void DisplayLinkValues();
-  
+
  protected:
 
 
@@ -63,9 +70,12 @@ class ControlOverGz {
   /// Name of state service
   std::string state_service_gzsim_;
 
-  /// Name of wrold control state service.
+  /// Name of world control state service.
   std::string wrld_ctrl_state_srv_gzsim_;
 
+  /// Name of set pose service.
+  std::string set_pose_service_gzsim_;
+
   /// GZ node
   gz::transport::Node node_;
 

src/control_over_gz.cc

@@ -25,6 +25,9 @@ ControlOverGz::ControlOverGz(std::string &world_prefix, bool debug_level)
   /// State gzsim service
   state_service_gzsim_ = world_prefix+std::string("/state");
 
+  /// Set pose service
+  set_pose_service_gzsim_ = world_prefix + std::string("/set_pose");
+
   std::string gazebo_clock_topic_name;
   gazebo_clock_topic_name = world_prefix + std::string("/clock");
   node_.Subscribe<ControlOverGz,gz::msgs::Clock>(gazebo_clock_topic_name,
@@ -152,7 +155,7 @@ bool ControlOverGz::ReadWorldStateToInitECM()
 void ControlOverGz::DisplayLinkValues()
 {
   auto entities = Robot_ECM_.EntitiesByComponents( gz::sim::components::Link());
-  
+
   for (auto it_entity = entities.begin();
        it_entity != entities.end();
        it_entity++)
@@ -220,6 +223,7 @@ bool ControlOverGz::SendWorldControlStateToInitECM(const RobotCtrlJointInfos &rb
     }
   }
 
+
   /// Create the serialized state.
   req_serialized_state = new gz::msgs::SerializedState();
   ///  Generate the serialized state msg from the ECM.
@@ -244,4 +248,36 @@ bool ControlOverGz::SendWorldControlStateToInitECM(const RobotCtrlJointInfos &rb
 
   return true;
 }
+
+bool ControlOverGz::SetPose(double x, double y, double z,
+                            double qx, double qy, double qz, double qw)
+{
+  gz::msgs::Pose pose_msg;
+  gz::msgs::Boolean rep_bool;
+  unsigned int timeout = 3000;
+  bool result;
+
+  gz::msgs::Vector3d * aPosition = pose_msg.mutable_position();
+  aPosition->set_x(x); aPosition->set_y(y); aPosition->set_z(z);
+
+  gz::msgs::Quaternion * aQuaternion  = pose_msg.mutable_orientation();
+  aQuaternion->set_x(qx); aQuaternion->set_y(qy); aQuaternion->set_z(qz); aQuaternion->set_w(qw);
+
+  /// Set Robot Pose
+  /// by calling the /prefix_world/set_pose Gazebo service.
+  if (!node_.Request(set_pose_service_gzsim_,
+                     pose_msg,
+                     timeout,
+                     rep_bool,
+                     result)) {
+
+    std::cerr << "Unable to send reset request ! timeout "
+              << wrld_ctrl_state_srv_gzsim_ << " "
+              << result
+              << std::endl;
+    result =false;
+  }
+
+  return true;
+}
 }