Skip to content

Commit

Permalink
Merge pull request #248 from azeey/6_to_7
Browse files Browse the repository at this point in the history 
Merge gz-launch6 ➡️  gz-launch7
  • Loading branch information
@azeey
azeey authored Mar 14, 2024
2 parents 34ac377 + 31821c4 commit 3a797f4
Show file tree
Hide file tree
Showing 4 changed files with 101 additions and 75 deletions.
48 changes: 36 additions & 12 deletions plugins/joy_to_twist/JoyToTwist.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,8 @@
#include <unistd.h>
#endif

#include <string>

#include <gz/msgs/joy.pb.h>
#include <gz/msgs/twist.pb.h>

Expand Down Expand Up @@ -49,6 +51,22 @@ void setVectorFromString(const std::string &_str,
}
}

//////////////////////////////////////////////////
// String to vector helper function.
void setVectorFromString(const std::string &_str,
gz::math::Vector3i &_v)
{
std::string str = gz::common::trimmed(_str);

std::vector<std::string> parts = gz::common::split(str, " ");
if (parts.size() == 3)
{
_v.X(std::stoi(parts[0]));
_v.Y(std::stoi(parts[1]));
_v.Z(std::stoi(parts[2]));
}
}

/////////////////////////////////////////////////
JoyToTwist::JoyToTwist()
: Plugin()
Expand Down Expand Up @@ -130,18 +148,21 @@ void JoyToTwist::OnJoy(const gz::msgs::Joy &_msg)
if (this->enableTurboButton >= 0 && _msg.buttons(this->enableTurboButton))
{
cmdVelMsg.mutable_linear()->set_x(
_msg.axes(this->axisLinear.X()) * this->scaleLinearTurbo.X());
_msg.axes(this->axisLinear.X()) * this->scaleLinearTurbo.X());
cmdVelMsg.mutable_linear()->set_y(
_msg.axes(this->axisLinear.Y()) * this->scaleLinearTurbo.Y());
_msg.axes(this->axisLinear.Y()) * this->scaleLinearTurbo.Y());
cmdVelMsg.mutable_linear()->set_z(
_msg.axes(this->axisLinear.Z()) * this->scaleLinearTurbo.Z());
_msg.axes(this->axisLinear.Z()) * this->scaleLinearTurbo.Z());

cmdVelMsg.mutable_angular()->set_x(
_msg.axes(this->axisAngular.X()) * this->scaleAngularTurbo.X());
_msg.axes(static_cast<int>(this->axisAngular.X())) *
this->scaleAngularTurbo.X());
cmdVelMsg.mutable_angular()->set_y(
_msg.axes(this->axisAngular.Y()) * this->scaleAngularTurbo.Y());
_msg.axes(static_cast<int>(this->axisAngular.Y())) *
this->scaleAngularTurbo.Y());
cmdVelMsg.mutable_angular()->set_z(
_msg.axes(this->axisAngular.Z()) * this->scaleAngularTurbo.Z());
_msg.axes(static_cast<int>(this->axisAngular.Z())) *
this->scaleAngularTurbo.Z());

this->cmdVelPub.Publish(cmdVelMsg);
this->sentDisableMsg = false;
Expand All @@ -150,18 +171,21 @@ void JoyToTwist::OnJoy(const gz::msgs::Joy &_msg)
else if (_msg.buttons(this->enableButton))
{
cmdVelMsg.mutable_linear()->set_x(
_msg.axes(this->axisLinear.X()) * this->scaleLinear.X());
_msg.axes(this->axisLinear.X()) * this->scaleLinear.X());
cmdVelMsg.mutable_linear()->set_y(
_msg.axes(this->axisLinear.Y()) * this->scaleLinear.Y());
_msg.axes(this->axisLinear.Y()) * this->scaleLinear.Y());
cmdVelMsg.mutable_linear()->set_z(
_msg.axes(this->axisLinear.Z()) * this->scaleLinear.Z());
_msg.axes(this->axisLinear.Z()) * this->scaleLinear.Z());

cmdVelMsg.mutable_angular()->set_x(
_msg.axes(this->axisAngular.X()) * this->scaleAngular.X());
_msg.axes(static_cast<int>(this->axisAngular.X())) *
this->scaleAngular.X());
cmdVelMsg.mutable_angular()->set_y(
_msg.axes(this->axisAngular.Y()) * this->scaleAngular.Y());
_msg.axes(static_cast<int>(this->axisAngular.Y())) *
this->scaleAngular.Y());
cmdVelMsg.mutable_angular()->set_z(
_msg.axes(this->axisAngular.Z()) * this->scaleAngular.Z());
_msg.axes(static_cast<int>(this->axisAngular.Z())) *
this->scaleAngular.Z());

this->cmdVelPub.Publish(cmdVelMsg);
this->sentDisableMsg = false;
Expand Down
2 changes: 1 addition & 1 deletion plugins/joy_to_twist/JoyToTwist.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,7 @@ namespace gz

private: int enableButton = 0;
private: int enableTurboButton = -1;
private: gz::math::Vector3d axisLinear{1.0, 0.0, 0.0};
private: gz::math::Vector3i axisLinear{1, 0, 0};
private: gz::math::Vector3d scaleLinear{0.5, 0.0, 0.0};
private: gz::math::Vector3d scaleLinearTurbo{0.5, 0.0, 0.0};

Expand Down
6 changes: 3 additions & 3 deletions src/Manager.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@
#include <memory>
#include <string>

// #include <gz/common/SuppressWarning.hh>
#include <gz/utils/SuppressWarning.hh>

#include <gz/launch/Export.hh>

Expand Down Expand Up @@ -52,9 +52,9 @@ namespace gz
public: bool Stop();

/// \brief Private data pointer.
// GZ_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING
GZ_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING
private: std::unique_ptr<ManagerPrivate> dataPtr;
// GZ_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING
GZ_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING
};
}
}
Expand Down
120 changes: 61 additions & 59 deletions src/vendor/backward.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,13 +51,13 @@
// You can define one of the following (or leave it to the auto-detection):
//
// #define BACKWARD_SYSTEM_LINUX
// - specialization for linux
// - specialization for linux
//
// #define BACKWARD_SYSTEM_DARWIN
// - specialization for Mac OS X 10.5 and later.
// - specialization for Mac OS X 10.5 and later.
//
// #define BACKWARD_SYSTEM_UNKNOWN
// - placebo implementation, does nothing.
// - placebo implementation, does nothing.
//
#if defined(BACKWARD_SYSTEM_LINUX)
#elif defined(BACKWARD_SYSTEM_DARWIN)
Expand Down Expand Up @@ -132,9 +132,9 @@
// - object filename
// - function name
// - source filename
// - line and column numbers
// - source code snippet (assuming the file is accessible)
// - variables name and values (if not optimized out)
// - line and column numbers
// - source code snippet (assuming the file is accessible)
// - variables name and values (if not optimized out)
// - You need to link with the lib "dw":
// - apt-get install libdw-dev
// - g++/clang++ -ldw ...
Expand All @@ -144,8 +144,8 @@
// - object filename
// - function name
// - source filename
// - line numbers
// - source code snippet (assuming the file is accessible)
// - line numbers
// - source code snippet (assuming the file is accessible)
// - You need to link with the lib "bfd":
// - apt-get install binutils-dev
// - g++/clang++ -lbfd ...
Expand Down Expand Up @@ -191,8 +191,8 @@
#include <cxxabi.h>
#include <fcntl.h>
#ifdef __ANDROID__
// Old Android API levels define _Unwind_Ptr in both link.h and
// unwind.h Rename the one in link.h as we are not going to be using
// Old Android API levels define _Unwind_Ptr in both link.h and
// unwind.h Rename the one in link.h as we are not going to be using
// it
#define _Unwind_Ptr _Unwind_Ptr_Custom
#include <link.h>
Expand Down Expand Up @@ -320,7 +320,9 @@
#include <BaseTsd.h>
typedef SSIZE_T ssize_t;

#ifndef NOMINMAX
#define NOMINMAX
#endif
#include <Windows.h>
#include <winnt.h>

Expand Down Expand Up @@ -1129,14 +1131,14 @@ class TraceResolverLinuxImpl<trace_resolver_tag::libbfd>

// Now we get in symbol_info:
// .dli_fname:
// pathname of the shared object that contains the address.
// pathname of the shared object that contains the address.
// .dli_fbase:
// where the object is loaded in memory.
// where the object is loaded in memory.
// .dli_sname:
// the name of the nearest symbol to trace.addr, we expect a
// function name.
// the name of the nearest symbol to trace.addr, we expect a
// function name.
// .dli_saddr:
// the exact address corresponding to .dli_sname.
// the exact address corresponding to .dli_sname.

if (symbol_info.dli_sname) {
trace.object_function = demangle(symbol_info.dli_sname);
Expand Down Expand Up @@ -1218,47 +1220,47 @@ class TraceResolverLinuxImpl<trace_resolver_tag::libbfd>
trace.inliners = backtrace_inliners(fobj, *details_selected);

#if 0
if (trace.inliners.size() == 0) {
// Maybe the trace was not inlined... or maybe it was and we
// are lacking the debug information. Let's try to make the
// world better and see if we can get the line number of the
// function (trace.source.function) now.
//
// We will get the location of where the function start (to be
// exact: the first instruction that really start the
// function), not where the name of the function is defined.
// This can be quite far away from the name of the function
// btw.
//
// If the source of the function is the same as the source of
// the trace, we cannot say if the trace was really inlined or
// not. However, if the filename of the source is different
// between the function and the trace... we can declare it as
// an inliner. This is not 100% accurate, but better than
// nothing.

if (symbol_info.dli_saddr) {
find_sym_result details = find_symbol_details(fobj,
symbol_info.dli_saddr,
symbol_info.dli_fbase);

if (details.found) {
ResolvedTrace::SourceLoc diy_inliner;
diy_inliner.line = details.line;
if (details.filename) {
diy_inliner.filename = details.filename;
}
if (details.funcname) {
diy_inliner.function = demangle(details.funcname);
} else {
diy_inliner.function = trace.source.function;
}
if (diy_inliner != trace.source) {
trace.inliners.push_back(diy_inliner);
}
}
}
}
if (trace.inliners.size() == 0) {
// Maybe the trace was not inlined... or maybe it was and we
// are lacking the debug information. Let's try to make the
// world better and see if we can get the line number of the
// function (trace.source.function) now.
//
// We will get the location of where the function start (to be
// exact: the first instruction that really start the
// function), not where the name of the function is defined.
// This can be quite far away from the name of the function
// btw.
//
// If the source of the function is the same as the source of
// the trace, we cannot say if the trace was really inlined or
// not. However, if the filename of the source is different
// between the function and the trace... we can declare it as
// an inliner. This is not 100% accurate, but better than
// nothing.

if (symbol_info.dli_saddr) {
find_sym_result details = find_symbol_details(fobj,
symbol_info.dli_saddr,
symbol_info.dli_fbase);

if (details.found) {
ResolvedTrace::SourceLoc diy_inliner;
diy_inliner.line = details.line;
if (details.filename) {
diy_inliner.filename = details.filename;
}
if (details.funcname) {
diy_inliner.function = demangle(details.funcname);
} else {
diy_inliner.function = trace.source.function;
}
if (diy_inliner != trace.source) {
trace.inliners.push_back(diy_inliner);
}
}
}
}
#endif
}

Expand Down Expand Up @@ -3434,9 +3436,9 @@ class SourceFile {
lines_t &get_lines(unsigned line_start, unsigned line_count, lines_t &lines) {
using namespace std;
// This function make uses of the dumbest algo ever:
// 1) seek(0)
// 2) read lines one by one and discard until line_start
// 3) read line one by one until line_start + line_count
// 1) seek(0)
// 2) read lines one by one and discard until line_start
// 3) read line one by one until line_start + line_count
//
// If you are getting snippets many time from the same file, it is
// somewhat a waste of CPU, feel free to benchmark and propose a
Expand Down

0 comments on commit 3a797f4

Please sign in to comment.