Skip to content

Commit

Permalink
VisualizeForces: remove unused code.
Browse files Browse the repository at this point in the history 
- Remove unused components.
- Remove debug / prototyping code from Link and VisualizeForces.

Signed-off-by: Rhys Mainwaring <[email protected]>
  • Loading branch information
@srmainwaring
srmainwaring committed Mar 23, 2023
1 parent d0594d3 commit fbcf946
Show file tree
Hide file tree
Showing 3 changed files with 2 additions and 298 deletions.
82 changes: 0 additions & 82 deletions include/gz/sim/components/EntityWrench.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,6 @@
#include <string>
#include <unordered_map>

#include <gz/msgs/entity_wrench.pb.h>
#include <gz/msgs/entity_wrench_map.pb.h>
#include <gz/sim/components/Component.hh>
#include <gz/sim/components/Factory.hh>
Expand All @@ -33,97 +32,16 @@ namespace sim
{
// Inline bracket to help doxygen filtering.
inline namespace GZ_SIM_VERSION_NAMESPACE {
namespace serializers
{
class EntityWrenchesSerializer
{
/// \brief Serialization for `unordered_map<string, msgs::EntityWrench>`
/// \param[in] _out Output stream.
/// \param[in] _data The data to stream.
/// \return The stream.
public: static std::ostream &Serialize(std::ostream &_out,
const std::unordered_map<std::string, msgs::EntityWrench> &_data)
{
_out << _data.size();
for (const auto& [key, value] : _data)
{
// serialize to string so we can determine size
std::string str = value.SerializeAsString();
_out << " " << key;
_out << " " << str.size();
_out << str;

// debug info
// gzdbg << value.DebugString() << "\n";
}
return _out;
}

/// \brief Deserialization for `unordered_set<string, msgs::EntityWrench>`
/// \param[in] _in Input stream.
/// \param[out] _data The data to populate.
/// \return The stream.
public: static std::istream &Deserialize(std::istream &_in,
std::unordered_map<std::string, msgs::EntityWrench> &_data)
{
size_t size;
_in >> size;
for (size_t i = 0; i < size; ++i)
{
// read key and msg size
std::string key;
size_t str_size;

_in >> key;
_in >> str_size;

// construct string buffer to required size
std::string str(str_size, '\0');
_in.read(&str[0], str_size);

// parse message
msgs::EntityWrench msg;
msg.ParseFromString(str);
_data.insert({key, msg});

// debug info
// gzdbg << msg.DebugString() << "\n";
}
return _in;
}
};
}

namespace components
{
/// \brief A component type that contains a wrench and an entity expressed
/// in the world frame to apply the wrench to. It is represented by
/// gz::msgs::EntityWrench.
/// The wrench uses SI units (N for force and N⋅m for torque).
using EntityWrench =
Component<msgs::EntityWrench, class EntityWrenchTag,
serializers::MsgSerializer>;
GZ_SIM_REGISTER_COMPONENT("gz_sim_components.EntityWrench",
EntityWrench)

/// \brief A component type that contains an map of wrenches
/// for an entity. The wrenches are represented by gz::msgs::EntityWrenchMap.
using EntityWrenchMap =
Component<msgs::EntityWrenchMap, class EntityWrenchMapTag,
serializers::MsgSerializer>;
GZ_SIM_REGISTER_COMPONENT("gz_sim_components.EntityWrenchMap",
EntityWrenchMap)

/// \brief A component type that contains an unordered map of wrenches
/// for an entity. The wrenches are represented by gz::msgs::EntityWrench.
/// The key is a string label.
using EntityWrenches =
Component<std::unordered_map<std::string, msgs::EntityWrench>,
class EntityWrenchesTag,
serializers::EntityWrenchesSerializer>;
GZ_SIM_REGISTER_COMPONENT("gz_sim_components.EntityWrenches",
EntityWrenches)

}
}
} // namespace sim
Expand Down
146 changes: 2 additions & 144 deletions src/Link.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,6 @@
#include <gz/math/Vector3.hh>

#include <gz/msgs/Utility.hh>
#include <gz/msgs/entity_wrench.pb.h>
#include <gz/msgs/entity_wrench_map.pb.h>

#include "gz/sim/components/AngularAcceleration.hh"
Expand Down Expand Up @@ -439,82 +438,6 @@ void Link::AddWorldWrench(EntityComponentManager &_ecm,
msgs::Convert(linkWrenchComp->Data().torque()) + _torque);
}

#if 0
// Publish components::EntityWrenches
if (this->dataPtr->visualizationLabel.has_value())
{
auto& label = this->dataPtr->visualizationLabel.value();

// Enable required components.
enableComponent<components::WorldPose>(_ecm, this->dataPtr->id, true);
enableComponent<components::EntityWrenches>(_ecm, this->dataPtr->id, true);

auto entityWrenchesComp =
_ecm.Component<components::EntityWrenches>(this->dataPtr->id);
if (!entityWrenchesComp)
{
static bool informed{false};
if (!informed)
{
gzerr << "Failed to retrieve EntityWrenches component for link ["
<< this->dataPtr->id << "] from [" << label << "]\n";
}
return;
}

// Populate data
msgs::EntityWrench msg;

// Set label
{
auto data = msg.mutable_header()->add_data();
data->set_key("label");
data->add_value(label);
}

// Set name
{
auto data = msg.mutable_header()->add_data();
data->set_key("name");
if (this->Name(_ecm).has_value())
{
data->add_value(this->Name(_ecm).value());
}
}

// Set entity
msg.mutable_entity()->set_id(this->Entity());

// Set wrench
msgs::Set(msg.mutable_wrench()->mutable_force(), _force);
msgs::Set(msg.mutable_wrench()->mutable_torque(), _torque);

// Update map with wrench
auto& data = entityWrenchesComp->Data();
if (data.find(label) == data.end())
{
data.insert({label, msg});
}
else
{
data.at(label) = msg;
}

_ecm.SetChanged(this->dataPtr->id, components::EntityWrenches::typeId,
ComponentState::PeriodicChange);

// {
// gzdbg << "Publishing entity wrenches for link ["
// << this->dataPtr->id << "]\n"
// << "Size: " << entityWrenchesComp->Data().size() << "\n"
// << "Label: " << label << "\n"
// << entityWrenchesComp->Data()[label].DebugString() << "\n";
// }
}
#endif

#if 1
// Publish components::EntityWrenchMap
if (this->dataPtr->visualizationLabel.has_value())
{
auto& label = this->dataPtr->visualizationLabel.value();
Expand Down Expand Up @@ -573,77 +496,12 @@ void Link::AddWorldWrench(EntityComponentManager &_ecm,
// {
// gzdbg << "Publishing entity wrench map for link ["
// << this->dataPtr->id << "]\n"
// << "Size: " << entityWrenchMapComp->Data().wrenches().size() << "\n"
// << "Size: "
// << entityWrenchMapComp->Data().wrenches().size() << "\n"
// << "Label: " << label << "\n"
// << entityWrenchMapComp->Data().DebugString() << "\n";
// }
}
#endif

#if 0
/// \todo(srmainwaring) - for debugging - publish components::EntityWrenches
if (this->dataPtr->visualizationLabel.has_value())
{
auto& label = this->dataPtr->visualizationLabel.value();

// Enable required components.
enableComponent<components::WorldPose>(_ecm, this->dataPtr->id, true);
enableComponent<components::EntityWrench>(_ecm, this->dataPtr->id, true);

auto entityWrenchComp =
_ecm.Component<components::EntityWrench>(this->dataPtr->id);
if (!entityWrenchComp)
{
static bool informed{false};
if (!informed)
{
gzerr << "Failed to retrieve EntityWrench component for link ["
<< this->dataPtr->id << "] from [" << label << "]\n";
}
return;
}

// Populate data
msgs::EntityWrench msg;

// Set label
{
auto data = msg.mutable_header()->add_data();
data->set_key("label");
data->add_value(label);
}

// Set name
{
auto data = msg.mutable_header()->add_data();
data->set_key("name");
if (this->Name(_ecm).has_value())
{
data->add_value(this->Name(_ecm).value());
}
}

// Set entity
msg.mutable_entity()->set_id(this->Entity());

// Set wrench
msgs::Set(msg.mutable_wrench()->mutable_force(), _force);
msgs::Set(msg.mutable_wrench()->mutable_torque(), _torque);

// Update map with wrench
entityWrenchComp->Data() = msg;

_ecm.SetChanged(this->dataPtr->id, components::EntityWrench::typeId,
ComponentState::PeriodicChange);

// {
// gzdbg << "Publishing entity wrench for link ["
// << this->dataPtr->id << "]\n"
// << "Label: " << label << "\n"
// << entityWrenchComp->Data().DebugString() << "\n";
// }
}
#endif
}

//////////////////////////////////////////////////
Expand Down
Loading

0 comments on commit fbcf946

Please sign in to comment.