Skip to content
Snippets Groups Projects
Commit 7c9eecd8 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

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

parent 580b5549
No related branches found
No related tags found
No related merge requests found
......@@ -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_;
......
......@@ -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;
}
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment