57 lines
2.1 KiB
C++
57 lines
2.1 KiB
C++
/**
|
|
* @file like.cpp
|
|
* @brief Robot holds a thumbs-up "like" WHILE the built-in handshake arm action plays.
|
|
*
|
|
* A background thread republishes the 👍 continuously (so inspire_g1 always has a
|
|
* fresh command and the fingers stay curled), while the main thread triggers the
|
|
* handshake and holds it for a fixed window (ExecuteAction can return before the
|
|
* arm actually finishes moving, so we don't gate on its return).
|
|
*
|
|
* ./like [thumb_bend] [thumb_rot] [networkInterface]
|
|
* thumb_bend, thumb_rot in [0,1] (default 1 1). Tweak if 👍 isn't clean:
|
|
* e.g. ./like 1 0 ./like 0.7 0.3
|
|
* default iface: eth0
|
|
*
|
|
* Needs exactly one ./inspire_g1 running for the fingers. The ARM moves —
|
|
* clear space, E-stop ready.
|
|
*/
|
|
#include "greet_common.hpp"
|
|
#include <cstdlib>
|
|
#include <thread>
|
|
#include <atomic>
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
const float tb = argc > 1 ? std::atof(argv[1]) : 1.0f; // thumb_bend (1 = extended)
|
|
const float tr = argc > 2 ? std::atof(argv[2]) : 1.0f; // thumb_rot
|
|
const std::string iface = argc > 3 ? argv[3] : "eth0";
|
|
|
|
auto arm = greet_init(iface);
|
|
Hand hand;
|
|
usleep(1500000); // 1.5s: let DDS match inspire_g1 + the arm service before commanding
|
|
|
|
// On these hands the 4 fingers close at q=1 (q=0 = extended, inverted vs SDK doc).
|
|
const Vec6 like = gv(1, 1, 1, 1, tb, tr); // 4 fingers curled into a fist, thumb up
|
|
|
|
std::cout << "*** LIKE while handshake (arm + hand). Clear space, E-stop ready. ***\n"
|
|
<< "hand: thumbs-up (thumb_bend=" << tb << " thumb_rot=" << tr << ")" << std::endl;
|
|
|
|
// Background thread: keep the 👍 published the whole time.
|
|
std::atomic<bool> running{true};
|
|
std::thread holder([&] { while (running) { hand.set(like, G_OPEN); ghold(40); } });
|
|
|
|
ghold(1200); // let the 👍 form before the arm starts
|
|
|
|
if (arm_do(*arm, 27, "shake hand")) {
|
|
ghold(4500); // hold the 👍 while the handshake plays
|
|
arm_do(*arm, 99, "release arm");
|
|
ghold(500);
|
|
}
|
|
|
|
running = false;
|
|
holder.join();
|
|
hand.set(G_OPEN, G_OPEN);
|
|
std::cout << "done." << std::endl;
|
|
return 0;
|
|
}
|