Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix various issues #1275

Merged
merged 9 commits into from
Nov 16, 2023
3 changes: 3 additions & 0 deletions ChangeLog.txt
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,9 @@ ViSP 3.x.x (Version in development)
. [#1251] Bug in vpDisplay::displayFrame()
. [#1270] Build issue around std::clamp and optional header which are not found with cxx17
standard enabled
. [#1272] Unable to build ViSP with PCL/VTK build from source, VTK headers are not found by ViSP
. [#1273] Build error in visp_java without deprecated functionalities
. [#1274] Build issue no member named clamp in namespace std
----------------------------------------------
ViSP 3.6.0 (released September 22, 2023)
- Contributors:
Expand Down
8 changes: 4 additions & 4 deletions example/servo-afma6/servoAfma6MegaposePBVS.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -77,8 +77,8 @@
#include <visp3/core/vpImageFilter.h>
#include <visp3/io/vpVideoWriter.h>


#if defined(VISP_HAVE_REALSENSE2) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \
// Check if std:c++17 or higher
#if defined(VISP_HAVE_REALSENSE2) && ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) && \
(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_MODULE_DNN_TRACKER)

#include <optional>
Expand Down Expand Up @@ -455,8 +455,8 @@ int main()
#if !defined(VISP_HAVE_REALSENSE2)
std::cout << "Install librealsense-2.x" << std::endl;
#endif
#if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_17)
std::cout << "Build ViSP with c++11 or higher compiler flag (cmake -DUSE_CXX_STANDARD=17)." << std::endl;
#if !((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
std::cout << "Build ViSP with c++17 or higher compiler flag (cmake -DUSE_CXX_STANDARD=17)." << std::endl;
#endif
#if !defined(VISP_HAVE_AFMA6)
std::cout << "ViSP is not built with Afma-6 robot support..." << std::endl;
Expand Down
138 changes: 77 additions & 61 deletions example/servo-pixhawk/sendMocapToPixhawk.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,8 @@

#include <visp3/core/vpConfig.h>

#if defined(VISP_HAVE_MAVSDK) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \
// Check if std:c++17 or higher
#if defined(VISP_HAVE_MAVSDK) && ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) && \
(defined(VISP_HAVE_QUALISYS) || defined(VISP_HAVE_VICON))

#include <chrono>
Expand Down Expand Up @@ -92,7 +93,8 @@ bool mocap_sdk_loop(std::mutex &lock, bool qualisys, bool opt_verbose, bool opt_
std::cout << "ERROR : Qualisys not found.";
return false;
#endif
} else {
}
else {
#ifdef VISP_HAVE_VICON
mocap = std::make_shared<vpMocapVicon>();
#else
Expand Down Expand Up @@ -120,19 +122,21 @@ bool mocap_sdk_loop(std::mutex &lock, bool qualisys, bool opt_verbose, bool opt_
if (!mocap->getBodiesPose(body_poses_enu_M_flu, opt_all_bodies)) {
g_quit = true;
}
} else {
}
else {
vpHomogeneousMatrix enu_M_flu;
if (!mocap->getSpecificBodyPose(opt_onlyBody, enu_M_flu)) {
g_quit = true;
} else {
}
else {
body_poses_enu_M_flu[opt_onlyBody] = enu_M_flu;
}
}

lock.lock();
internal_mavlink_failure = mavlink_failure;
current_body_poses_enu_M_flu =
body_poses_enu_M_flu; // Now we send directly the poses in the ENU global reference frame.
body_poses_enu_M_flu; // Now we send directly the poses in the ENU global reference frame.
lock.unlock();
}
return true;
Expand All @@ -148,7 +152,7 @@ int top(const std::string &connection_info, std::map<std::string, vpHomogeneousM
bool internal_mocap_failure = false;
const double fps = 100;

vpRobotMavsdk drone{connection_info};
vpRobotMavsdk drone { connection_info };

while (!g_quit && !internal_mocap_failure) {
double t = vpTime::measureTimeMs();
Expand Down Expand Up @@ -176,44 +180,44 @@ int top(const std::string &connection_info, std::map<std::string, vpHomogeneousM
void usage(char *argv[], int error)
{
std::cout << "SYNOPSIS" << std::endl
<< " " << argv[0] << " [--only-body <name>] [-ob]"
<< " [--mocap-system <qualisys>/<vicon>] [-ms <q>/<v>]"
<< " [--device <device port>] [-d]"
<< " [--server-address <server address>] [-sa]"
<< " [--verbose] [-v]"
<< " [--help] [-h]" << std::endl
<< std::endl;
<< " " << argv[0] << " [--only-body <name>] [-ob]"
<< " [--mocap-system <qualisys>/<vicon>] [-ms <q>/<v>]"
<< " [--device <device port>] [-d]"
<< " [--server-address <server address>] [-sa]"
<< " [--verbose] [-v]"
<< " [--help] [-h]" << std::endl
<< std::endl;
std::cout << "DESCRIPTION" << std::endl
<< "MANDATORY PARAMETERS :" << std::endl
<< " --only-body <name>" << std::endl
<< " Name of the specific body you want to be displayed." << std::endl
<< std::endl
<< "OPTIONAL PARAMETERS (DEFAULT VALUES)" << std::endl
<< " --mocap-system, -ms" << std::endl
<< " Specify the name of the mocap system : 'qualisys' / 'q' or 'vicon'/ 'v'." << std::endl
<< " Default: Qualisys mode." << std::endl
<< std::endl
<< " --device <device port>, -d" << std::endl
<< " String giving us all the informations necessary for connection." << std::endl
<< " Default: serial:///dev/ttyUSB0 ." << std::endl
<< " UDP example: udp://192.168.30.111:14540 (udp://IP:Port) ." << std::endl
<< std::endl
<< " --server-address <address>, -sa" << std::endl
<< " Mocap server address." << std::endl
<< " Default for Qualisys: 192.168.34.42 ." << std::endl
<< " Default for Vicon: 192.168.34.1 ." << std::endl
<< std::endl
<< " --verbose, -v" << std::endl
<< " Enable verbose mode." << std::endl
<< std::endl
<< " --help, -h" << std::endl
<< " Print this helper message." << std::endl
<< std::endl;
<< "MANDATORY PARAMETERS :" << std::endl
<< " --only-body <name>" << std::endl
<< " Name of the specific body you want to be displayed." << std::endl
<< std::endl
<< "OPTIONAL PARAMETERS (DEFAULT VALUES)" << std::endl
<< " --mocap-system, -ms" << std::endl
<< " Specify the name of the mocap system : 'qualisys' / 'q' or 'vicon'/ 'v'." << std::endl
<< " Default: Qualisys mode." << std::endl
<< std::endl
<< " --device <device port>, -d" << std::endl
<< " String giving us all the informations necessary for connection." << std::endl
<< " Default: serial:///dev/ttyUSB0 ." << std::endl
<< " UDP example: udp://192.168.30.111:14540 (udp://IP:Port) ." << std::endl
<< std::endl
<< " --server-address <address>, -sa" << std::endl
<< " Mocap server address." << std::endl
<< " Default for Qualisys: 192.168.34.42 ." << std::endl
<< " Default for Vicon: 192.168.34.1 ." << std::endl
<< std::endl
<< " --verbose, -v" << std::endl
<< " Enable verbose mode." << std::endl
<< std::endl
<< " --help, -h" << std::endl
<< " Print this helper message." << std::endl
<< std::endl;

if (error) {
std::cout << "Error" << std::endl
<< " "
<< "Unsupported parameter " << argv[error] << std::endl;
<< " "
<< "Unsupported parameter " << argv[error] << std::endl;
}
}

Expand All @@ -230,31 +234,40 @@ void parse_commandline(int argc, char **argv, bool &qualisys, std::string &conne
if (std::string(argv[i]) == "--only-body" || std::string(argv[i]) == "-ob") {
only_body = std::string(argv[i + 1]);
i++;
} else if (std::string(argv[i]) == "--mocap-system" || std::string(argv[i]) == "-ms") {
}
else if (std::string(argv[i]) == "--mocap-system" || std::string(argv[i]) == "-ms") {
std::string mode = std::string(argv[i + 1]);
if (mode == "qualisys" || mode == "q") {
qualisys = true;
} else if (mode == "vicon" || mode == "v") {
}
else if (mode == "vicon" || mode == "v") {
qualisys = false;
} else {
}
else {
std::cout << "ERROR : System not recognized, exiting." << std::endl;
throw EXIT_FAILURE;
}
i++;
} else if (std::string(argv[i]) == "--device" || std::string(argv[i]) == "-d") {
}
else if (std::string(argv[i]) == "--device" || std::string(argv[i]) == "-d") {
connection_info = std::string(argv[i + 1]);
i++;
} else if (std::string(argv[i]) == "--server-address" || std::string(argv[i]) == "-sa") {
}
else if (std::string(argv[i]) == "--server-address" || std::string(argv[i]) == "-sa") {
server_address = std::string(argv[i + 1]);
i++;
} else if (std::string(argv[i]) == "--all-bodies") {
}
else if (std::string(argv[i]) == "--all-bodies") {
all_bodies = true;
} else if (std::string(argv[i]) == "--verbose" || std::string(argv[i]) == "-v") {
}
else if (std::string(argv[i]) == "--verbose" || std::string(argv[i]) == "-v") {
verbose = true;
} else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
}
else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
usage(argv, 0);
throw EXIT_SUCCESS;
} else {
}
else {
usage(argv, i);
throw EXIT_FAILURE;
}
Expand Down Expand Up @@ -289,7 +302,8 @@ int main(int argc, char **argv)

if (opt_qualisys && opt_serverAddress == "") {
opt_serverAddress = "192.168.30.42";
} else if (!opt_qualisys && opt_serverAddress == "") {
}
else if (!opt_qualisys && opt_serverAddress == "") {
opt_serverAddress = "192.168.30.1";
}

Expand All @@ -304,8 +318,8 @@ int main(int argc, char **argv)
bool mavlink_failure = false;
std::thread mocap_thread([&lock, &opt_qualisys, &opt_verbose, &opt_all_bodies, &opt_serverAddress, &opt_onlyBody,
&current_body_poses_enu_M_flu, &mocap_failure, &mavlink_failure]() {
mocap_sdk_loop(lock, opt_qualisys, opt_verbose, opt_all_bodies, opt_serverAddress, opt_onlyBody,
current_body_poses_enu_M_flu, mocap_failure, mavlink_failure);
mocap_sdk_loop(lock, opt_qualisys, opt_verbose, opt_all_bodies, opt_serverAddress, opt_onlyBody,
current_body_poses_enu_M_flu, mocap_failure, mavlink_failure);
});
if (mocap_failure) {
std::cout << "Mocap connexion failure. Check mocap server IP address" << std::endl;
Expand All @@ -318,7 +332,8 @@ int main(int argc, char **argv)
try {
int result = top(opt_connectionInfo, current_body_poses_enu_M_flu, lock, mocap_failure);
return result;
} catch (int error) {
}
catch (int error) {
fprintf(stderr, "mavlink_control threw exception %i \n", error);
lock.lock();
mavlink_failure = true;
Expand All @@ -331,7 +346,8 @@ int main(int argc, char **argv)
mavlink_thread.join();
if (mocap_failure) {
return EXIT_FAILURE;
} else {
}
else {
return EXIT_SUCCESS;
}
}
Expand All @@ -342,18 +358,18 @@ int main()
{
#ifndef VISP_HAVE_MAVSDK
std::cout << "\nThis example requires mavsdk library. You should install it, configure and rebuid ViSP.\n"
<< std::endl;
<< std::endl;
#endif
#if !(defined(VISP_HAVE_QUALISYS) || defined(VISP_HAVE_VICON))
std::cout << "\nThis example requires data from a Qualisys or Vicon mocap system. You should install it, configure "
"and rebuid ViSP.\n"
<< std::endl;
"and rebuid ViSP.\n"
<< std::endl;
#endif
#if !(VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17)
#if !((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
std::cout
<< "\nThis example requires at least cxx17. You should enable cxx17 during ViSP configuration with cmake and "
"rebuild ViSP.\n"
<< std::endl;
<< "\nThis example requires at least cxx17. You should enable cxx17 during ViSP configuration with cmake and "
"rebuild ViSP.\n"
<< std::endl;
#endif
return EXIT_SUCCESS;
}
Expand Down
Loading
Loading