diff --git a/humanPose/CMakeLists.txt b/humanPose/CMakeLists.txt new file mode 100644 index 0000000..3827305 --- /dev/null +++ b/humanPose/CMakeLists.txt @@ -0,0 +1,18 @@ +# requires minimum cmake version +cmake_minimum_required(VERSION 3.16.0) + +# produce the cmake var PROJECT_NAME +project(edpr-hpe) + +find_package(YARP COMPONENTS os sig cv REQUIRED) +find_package(hpe-core REQUIRED) +find_package(event-driven REQUIRED) + +# final isaac application +add_executable(${PROJECT_NAME} ${PROJECT_NAME}.cpp) +target_link_libraries(${PROJECT_NAME} YARP::YARP_OS + YARP::YARP_sig + YARP::YARP_cv + ev::event-driven + hpe-core::hpe-core) +install(TARGETS ${PROJECT_NAME} DESTINATION ${CMAKE_INSTALL_BINDIR}) \ No newline at end of file diff --git a/humanPose/Dockerfile b/humanPose/Dockerfile new file mode 100644 index 0000000..7a5b6af --- /dev/null +++ b/humanPose/Dockerfile @@ -0,0 +1,178 @@ + +# base image +FROM nvidia/cuda:11.5.2-cudnn8-devel-ubuntu20.04 + +ENV DEBIAN_FRONTEND noninteractive + +# install basic indpendence +RUN apt update +RUN apt install -y build-essential libssl-dev software-properties-common +RUN apt install -y cmake cmake-curses-gui vim nano git sudo openssh-client git +RUN apt install -y libboost-all-dev libmysqlclient-dev ffmpeg libsm6 libxext6 libcanberra-gtk-module + +########## +# PYTHON & PIP # +########## + +# update python +ARG PYTHON_VERSION=3.8 +RUN apt install -y python$PYTHON_VERSION python3-pip python3-dev + +# create list of alternative python interpreters +RUN update-alternatives --install /usr/bin/python3 python3 /usr/bin/python$PYTHON_VERSION 1 && \ + update-alternatives --config python3 && \ + rm /usr/bin/python3 && \ + ln -s python$PYTHON_VERSION /usr/bin/python3 + +RUN pip3 install numpy + +# RUN python3 -m pip install scikit-build matplotlib jupyter pandas + +# RUN python3 -m pip install --upgrade pip + +#RUN apt install -y python3.7 \ +# python3-tk \ + +########## +# OPENCV C++ and Python +########## +RUN apt install -y libopencv-dev python3-opencv + +#RUN pip3 install opencv-python + +# install additional openpose dependencies + +#libhdf5-dev libatlas-base-dev + + +###################### +# set github ssh keys # +####################### + +ARG ssh_prv_key +ARG ssh_pub_key + +# Authorize SSH Host +RUN mkdir -p /root/.ssh && \ + chmod 0700 /root/.ssh +RUN ssh-keyscan github.com > /root/.ssh/known_hosts + +# Add the keys and set permissions +RUN echo "$ssh_prv_key" > /root/.ssh/id_ed25519 && \ + echo "$ssh_pub_key" > /root/.ssh/id_ed25519.pub && \ + chmod 600 /root/.ssh/id_ed25519 && \ + chmod 600 /root/.ssh/id_ed25519.pub + + +############### +# NEUROMORHPIC CAMERA DRIVER # +############### +RUN add-apt-repository ppa:deadsnakes/ppa +RUN echo "deb [arch=amd64 trusted=yes] https://apt.prophesee.ai/dists/public/7l58osgr/ubuntu focal essentials" >> /etc/apt/sources.list; +RUN apt update +RUN apt install -y python3.7 +RUN apt install -y metavision-* + +############ +# INSTALLED FROM SOURCE # +############ + +ARG SOURCE_FOLDER=/usr/local/src +ARG BUILD_TYPE=Release + +ARG YCM_VERSION=v0.15.1 +ARG YARP_VERSION=v3.7.2 +ARG EVENT_DRIVEN_VERSION=master +ARG HPE_VERSION=main + +RUN apt update + +RUN apt install -y \ + apt-transport-https \ + ca-certificates \ + gnupg \ + lsb-core \ + swig + +# Install yarp dependencies +RUN apt install -y \ + libgsl-dev \ + libedit-dev \ + libace-dev \ + libeigen3-dev \ +# Install QT5 for GUIS +# (NOTE: may be not compatible with nvidia drivers when forwarding screen) + qtbase5-dev \ + qt5-default \ + qtdeclarative5-dev \ + qtmultimedia5-dev \ + qml-module-qtquick2 \ + qml-module-qtquick-window2 \ + qml-module-qtmultimedia \ + qml-module-qtquick-dialogs \ + qml-module-qtquick-controls + +# git clone --depth 1 --branch url +# Install YCM +RUN cd $SOURCE_FOLDER && \ + git clone --depth 1 --branch $YCM_VERSION https://github.com/robotology/ycm.git &&\ + cd ycm && mkdir build && cd build && \ + cmake .. && make install -j$(nproc) + +# Install YARP +RUN cd $SOURCE_FOLDER && \ + git clone --depth 1 --branch $YARP_VERSION https://github.com/robotology/yarp.git &&\ + cd yarp && mkdir build && cd build && \ + cmake -DCMAKE_BUILD_TYPE=$BUILD_TYPE \ + -DYARP_COMPILE_BINDINGS=ON \ + -DCREATE_PYTHON=ON \ + .. && \ + make install -j$(nproc) + +RUN yarp check +EXPOSE 10000/tcp 10000/udp + +# make yarp's python binding visible to python interpreter +ENV PYTHONPATH $SOURCE_FOLDER/yarp/build/lib/python3:$PYTHONPATH + +# Some QT-Apps don't show controls without this +ENV QT_X11_NO_MITSHM 1 + + +# install EVENT-DRIVEN +RUN cd $SOURCE_FOLDER && \ + git clone --depth 1 --branch $EVENT_DRIVEN_VERSION https://github.com/robotology/event-driven.git && \ + cd event-driven && mkdir build && cd build && \ + cmake -DVLIB_ENABLE_TS=OFF .. && make install -j$(nproc) + +# install hpe-core +RUN cd $SOURCE_FOLDER && \ + git clone --depth 1 --branch $HPE_VERSION git@github.com:event-driven-robotics/hpe-core.git &&\ + cd hpe-core/core && mkdir build && cd build && \ + cmake .. && make install -j$(nproc) + +# add /usr/local/lib to the library path, so that libcaffe.so compiled with openpose will be used +# instead of the one provided by the nvidia/cuda docker image +#ENV LD_LIBRARY_PATH /usr/local/lib:$LD_LIBRARY_PATH + +# install movenet dependencies +RUN python3 -m pip install -r $SOURCE_FOLDER/hpe-core/example/movenet/requirements.txt + +ENV PYTHONPATH "${PYTHONPATH}:$SOURCE_FOLDER/hpe-core" + +#RUN export PYTHONPATH=$PYTHONPATH:$SOURCE_FOLDER/hpe-core +#ENV PYTHONPATH "${PYTHONPATH}:$SOURCE_FOLDER/hpe-core/example/movenet" + +# HPE application +RUN ssh-keyscan github.com > /root/.ssh/known_hosts +RUN cd $SOURCE_FOLDER && \ + git clone --branch main git@github.com:event-driven-robotics/event-driven-demos.git && \ + cd even-driven-demos && \ + git checkout hpe && cd humanPose && \ + mkdir build && cd build && \ + cmake .. && make install -j$(nproc) + +RUN apt autoremove && apt clean +RUN rm -rf /tmp/* /var/lib/apt/lists/* /var/tmp/* + +WORKDIR $SOURCE_FOLDER diff --git a/humanPose/README.MD b/humanPose/README.MD new file mode 100644 index 0000000..d9356ea --- /dev/null +++ b/humanPose/README.MD @@ -0,0 +1,68 @@ +# EDPR-HPE + +Human Pose Estimation for Ergonomics and Fatigue Detection + + +*Primary components:* +- Can use detectors: moveEnet +- Pixelwise velocity estimation +- Fusion of position and velocity estimation for latency compensated final position output + +The application has been designed to run using docker for simple set-up of the environment. + +## Installation +The software was tested on Ubuntu 20.04.2 LTS with an Nvidia GPU. + +- Install the latest [Nvidia driver](https://github.com/NVIDIA/nvidia-docker/wiki/Frequently-Asked-Questions#how-do-i-install-the-nvidia-driver) +- Install [Docker Engine](https://docs.docker.com/engine/install/ubuntu) +- Install [Nvidia Docker Toolkit](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/install-guide.html#docker) +- Download the repository and build the Docker image + ```shell + $ docker build -t event-pose - < Dockerfile + ``` +:bulb: `` is the parent directory in which the repository is cloned + +:bulb: The ssh keys are required to access and build this repository from within the docker, as it is currently private. [Create a new ssh key](https://docs.github.com/en/github/authenticating-to-github/connecting-to-github-with-ssh/generating-a-new-ssh-key-and-adding-it-to-the-ssh-agent) if required. + +:warning: Ensure your ssh key is built without a passphrase. + +:bulb: Your ssh key may have a different name (e.g. id_ed25519.pub). Change in the command as necessary. + +## Usage +- Run the Docker container and, inside it, run the pose detector + ```shell + $ xhost + + $ docker run -it --privileged --network host -v /tmp/.X11-unix/:/tmp/.X11-unix -v /dev/bus/usb:/dev/bus/usb -e DISPLAY=unix$DISPLAY --runtime=nvidia --name event-pose event-pose + ``` + +:bulb: use `-v :` to mount further folders inside the container in order to transfer data between your local machine and the container, if needed. + +- At the terminal inside the container run the following command to execute a script that will set up a yarpserver connected to ROS, run the atis-bridge to get the events from the camera and finally run the HPE application with a visualisation window. The application should automatically connect to required input and output YARP modules. This script also sets the value to the env variable `ROS_MASTER_URI`. It is either set to the default valaue (i.e. `http://127.0.0.1:11311`) using the `-d` flag, or manually set to a differnet value using the flag `-m`. + ```shell + $ ./run_hpe [-d (default)] or [-m value (manual)] + ``` + + +- The application should run with default parameters, using moveEnet detector at 10 Hz, without the latency compensation. + +- While the visualisation window is selected: + - Press `d` to visualise the detections (on and off) + - Press `v` to visualise the joint velocities (on and off) + - Press `e` to visualise alternate representations (toggle between eros and events) + - Press `ESC` to close + +- The following command line options can be used `edpr-hpe --OPTION VALUE` + - `--use_lc` to use latency compensation in the fusion + - `--detF ` to modify the moveEnet detection rate [10] + - `--w --h ` to set the dataset sensor resolution [640 480] + - `--roi ` to set the joint region-of-interest [20] + - `--pu --muD ` to set the Kalman filter process and measurement uncertainty [0.001, 0.0004] + - `--sc ` to change a tuning parameter associated with velocity [10] + - `--filepath ` to save a `.csv` of joint positions at a provided file location + - `--velpath ` to save a `.csv` of joint velocities at a provided file location + - `--v ` to save a video at a provided file location + +To stop the application itself, the camera and the yarpserver, the follwoing command should be run in a temrinal inside the docker: +```shell +$ ./kill_hpe +``` diff --git a/humanPose/edpr-hpe.cpp b/humanPose/edpr-hpe.cpp new file mode 100644 index 0000000..87523e1 --- /dev/null +++ b/humanPose/edpr-hpe.cpp @@ -0,0 +1,424 @@ +/* +Author: Franco Di Pietro, Arren Glover + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "hpe_msgs/NC_humanPose.h" + +using namespace yarp::os; +using namespace yarp::sig; +using std::vector; + +class externalDetector +{ +private: + + double period{0.1}, tic{0.0}; + bool waiting{false}; + + BufferedPort > output_port; + BufferedPort input_port; + +public: + + bool init(std::string output_name, std::string input_name, double rate) + { + if(!output_port.open(output_name)) + return false; + + if(!input_port.open(input_name)) + return false; + + period = 1.0 / rate; + return true; + } + void close() + { + output_port.close(); + input_port.close(); + } + + bool update(cv::Mat latest_image, double latest_ts, hpecore::stampedPose &previous_skeleton) + { + //send an update if the timer has elapsed + if((!waiting && latest_ts - tic > period) || (latest_ts - tic > 2.0)) + { + static cv::Mat cv_image; + cv::GaussianBlur(latest_image, cv_image, cv::Size(5, 5), 0, 0); + output_port.prepare().copy(yarp::cv::fromCvMat(cv_image)); + output_port.write(); + tic = latest_ts; + waiting = true; + } + + //read a ready data + Bottle *mn_container = input_port.read(false); + if (mn_container) + { + previous_skeleton.pose = hpecore::extractSkeletonFromYARP(*mn_container); + previous_skeleton.timestamp = tic; + previous_skeleton.delay = latest_ts - tic; + waiting = false; + } + + return mn_container != nullptr; + } +}; + + +class EDPR_HPE : public RFModule +{ + +private: + + //event reading + std::thread asynch_thread; + ev::window input_events; + + //detection handlers + externalDetector mn_handler; + + //velocity and fusion + hpecore::pwvelocity pw_velocity; + hpecore::multiJointLatComp state; + + //internal data structures + hpecore::skeleton13 skeleton_gt{0}; + hpecore::skeleton13 skeleton_detection{0}; + + cv::Size image_size; + cv::Mat vis_image; + cv::Mat edpr_logo; + + //recording results + hpecore::writer skelwriter, velwriter; + cv::VideoWriter output_video; + + //parameters + bool movenet{false}; + int detF{10}, roiSize{20}; + double scaler{12.5}; + bool alt_view{false}, pltVel{false}, pltDet{false}; + bool latency_compensation{true}; + double th_period{0.01}; + + // ros + yarp::os::Node* ros_node{nullptr}; + yarp::os::Publisher ros_publisher; + yarp::rosmsg::NC_humanPose ros_output; + +public: + bool configure(yarp::os::ResourceFinder &rf) override + { + // =====SET UP YARP===== + if (!yarp::os::Network::checkNetwork(2.0)) + { + std::cout << "Could not connect to YARP" << std::endl; + return false; + } + + // set the module name used to name ports + setName((rf.check("name", Value("/edpr_hpe")).asString()).c_str()); + + if (!input_events.open(getName("/AE:i"))) + { + yError() << "Could not open events input port"; + return false; + } + + // =====READ PARAMETERS===== + movenet = true; + + alt_view = rf.check("alt_view") && rf.check("alt_view", Value(true)).asBool(); + detF = rf.check("detF", Value(10)).asInt32(); + + image_size = cv::Size(rf.check("w", Value(640)).asInt32(), + rf.check("h", Value(480)).asInt32()); + roiSize = rf.check("roi", Value(20)).asInt32(); + + double procU = rf.check("pu", Value(1e-3)).asFloat64(); + double measUD = rf.check("muD", Value(1e-4)).asFloat64(); + double measUV = rf.check("muV", Value(0)).asFloat64(); + latency_compensation = rf.check("use_lc") && rf.check("use_lc", Value(true)).asBool(); + double lc = latency_compensation ? 1.0 : 0.0; + scaler = rf.check("sc", Value(12.5)).asFloat64(); + + // ===== SET UP DETECTOR METHOD ===== + if (movenet) { + // run python code for movenet + int r = system("python3 /usr/local/src/hpe-core/example/movenet/movenet_online.py &"); + while (!yarp::os::NetworkBase::exists("/movenet/sklt:o")) + sleep(1); + yInfo() << "MoveEnet started correctly"; + if (!mn_handler.init(getName("/eros:o"), getName("/movenet:i"), detF)) { + yError() << "Could not open movenet ports"; + return false; + } + } + + // ===== SET UP INTERNAL VARIABLE/DATA STRUCTURES ===== + + //shared images + vis_image = cv::Mat(image_size, CV_8UC3, cv::Vec3b(0, 0, 0)); + edpr_logo = cv::imread("/usr/local/src/event-driven-demos/humanPose/edpr_logo.png"); + + //velocity estimation + pw_velocity.setParameters(image_size, 7, 0.3, 0.01); + + //fusion + if(!state.initialise({procU, measUD, measUV, lc})) + { + yError() << "Not KF initialized"; + return false; + } + + // ===== SET UP RECORDING ===== + if (rf.check("filepath")) + { + std::string filepath = rf.find("filepath").asString(); + if (skelwriter.open(filepath)) + yInfo() << "saving data to:" << filepath; + } + if (rf.check("velpath")) + { + std::string velpath = rf.find("velpath").asString(); + if (velwriter.open(velpath)) + yInfo() << "saving velocity data to:" << velpath; + } + + if (rf.check("v")) { + std::string videopath = rf.find("v").asString(); + if (!output_video.open(videopath, + cv::VideoWriter::fourcc('H', '2', '6', '4'), + (int)(0.1/th_period), + image_size, + true)) { + yError() << "Could not open video writer!!"; + return false; + } + } + + // ===== TRY DEFAULT CONNECTIONS ===== + Network::connect("/file/ch0dvs:o", getName("/AE:i"), "fast_tcp"); + Network::connect("/atis3/AE:o", getName("/AE:i"), "fast_tcp"); + Network::connect("/file/ch2GT50Hzskeleton:o", getName("/gt:i"), "fast_tcp"); + Network::connect("/movenet/sklt:o", getName("/movenet:i"), "fast_tcp"); + Network::connect("/zynqGrabber/AE:o", getName("/AE:i"), "fast_tcp"); + Network::connect(getName("/eros:o"), "/movenet/img:i", "fast_tcp"); + + cv::namedWindow("edpr-hpe", cv::WINDOW_NORMAL); + cv::resizeWindow("edpr-hpe", cv::Size(640, 480)); + + + // set-up ROS interface + ros_node = new yarp::os::Node("/HPE"); + if(!ros_publisher.topic("/isim/neuromorphic_camera/data")) { + yError() << "Could not open ROS output publisher"; + return false; + } + else + yInfo() << "ROS output publisher: OK"; + + asynch_thread = std::thread([this]{ this->run_opixels(); }); + + return true; + } + + double getPeriod() override + { + // run the module as fast as possible. Only as fast as new images are + // available and then limited by how fast OpenPose takes to run + return th_period; + } + + bool interruptModule() override + { + // if the module is asked to stop ask the asynchronous thread to stop + input_events.stop(); + mn_handler.close(); + skelwriter.close(); + velwriter.close(); + output_video.release(); + asynch_thread.join(); + return true; + } + + bool close() override + { + // when the asynchronous thread is asked to stop, close ports and do other clean up + return true; + } + + void drawEROS(cv::Mat img) + { + cv::Mat eros8; + pw_velocity.queryEROS().convertTo(eros8, CV_8U, 255); + cv::cvtColor(eros8, img, cv::COLOR_GRAY2BGR); + cv::GaussianBlur(img, img, cv::Size(5, 5), 0, 0); + } + + // synchronous thread + bool updateModule() override + { + static cv::Mat canvas = cv::Mat(image_size, CV_8UC3); + canvas.setTo(cv::Vec3b(0, 0, 0)); + + //plot the image + //check if we plot events or alternative (PIM or EROS) + if(alt_view) + drawEROS(canvas); + else //events + vis_image.copyTo(canvas); + + vis_image.setTo(cv::Vec3b(0, 0, 0)); + + //plot skeletons + if(pltDet) + hpecore::drawSkeleton(canvas, skeleton_detection, {255, 0, 0}, 3); + + hpecore::drawSkeleton(canvas, state.query(), {0, 0, 255}, 3); + + if(pltVel) + { + hpecore::skeleton13_vel jv = state.queryVelocity(); + for (int j = 0; j < 13; j++) // (F) overload * to skeleton13 + jv[j] = jv[j] * 0.1; + + hpecore::drawVel(canvas, state.query(), jv, {255, 255, 102}, 2); + } + + if (!edpr_logo.empty()) { + static cv::Mat mask; + cv::cvtColor(edpr_logo, mask, CV_BGR2GRAY); + edpr_logo.copyTo(canvas, mask); + } + + if (output_video.isOpened()) + output_video << canvas; + + cv::imshow("edpr-hpe", canvas); + char key_pressed = cv::waitKey(10); + if(key_pressed > 0) + { + switch(key_pressed) + { + case 'v': + pltVel = !pltVel; break; + case 'd': + pltDet = !pltDet; break; + case 'e': + alt_view = !alt_view; break; + case '\e': + stopModule(); break; + + } + } + return true; + } + + void run_opixels() + { + hpecore::skeleton13_vel jv; + hpecore::skeleton13_vel skel_vel = {0}; + ev::info event_stats = {0}; + double latency = 0.0; + hpecore::stampedPose detected_pose; + input_events.readPacket(true); + double t0 = Time::now(); + std:vector sklt_out, vel_out; + + while (!isStopping()) + { + double tnow = Time::now() - t0; + + // ---------- DETECTIONS ---------- + bool was_detected = false; + if (movenet) { + static cv::Mat eros8; + pw_velocity.queryEROS().convertTo(eros8, CV_8U, 255); + was_detected = mn_handler.update(eros8, tnow, detected_pose); + } + + if (was_detected && hpecore::poseNonZero(detected_pose.pose)) { + skeleton_detection = detected_pose.pose; + latency = detected_pose.delay; + if (state.poseIsInitialised()) + state.updateFromPosition(skeleton_detection, detected_pose.timestamp); + else + state.set(skeleton_detection, tnow); + } + + // ---------- VELOCITY ---------- + //read events + event_stats = input_events.readAll(false); + if(event_stats.count == 0) + continue; + + //update images + for (auto &v : input_events) + { + if (v.p) + vis_image.at(v.y, v.x) = cv::Vec3b(64, 150, 90); + else + vis_image.at(v.y, v.x) = cv::Vec3b(32, 82, 50); + } + + pw_velocity.update(input_events.begin(), input_events.end(), event_stats.timestamp); + + //only update velocity if the pose is initialised + if(!state.poseIsInitialised()) + continue; + + skel_vel = pw_velocity.query(state.query(), 20, 2, state.queryVelocity()); + + //this scaler was thought to be from timestamp misconversion. + //instead we aren't sure why it is needed. + for (int j = 0; j < 13; j++) // (F) overload * to skeleton13 + skel_vel[j] = skel_vel[j] * scaler; + + state.setVelocity(skel_vel); + state.updateFromVelocity(skel_vel, event_stats.timestamp); + + skelwriter.write({event_stats.timestamp, latency, state.query()}); + velwriter.write({event_stats.timestamp, latency, skel_vel}); + + // format skeleton to ros output + sklt_out.clear(); + vel_out.clear(); + for (int j = 0; j < 13; j++) + { + sklt_out.push_back(skeleton_detection[j].u); + sklt_out.push_back(skeleton_detection[j].v); + vel_out.push_back(skel_vel[j].u); + vel_out.push_back(skel_vel[j].v); + } + ros_output.timestamp = event_stats.timestamp; + ros_output.pose = sklt_out; + ros_output.velocity = vel_out; + // publish data + ros_publisher.prepare() = ros_output; + ros_publisher.write(); + } + } +}; + +int main(int argc, char *argv[]) +{ + /* prepare and configure the resource finder */ + yarp::os::ResourceFinder rf; + rf.setVerbose(false); + rf.configure(argc, argv); + + /* create the module */ + EDPR_HPE instance; + return instance.runModule(rf); +} diff --git a/humanPose/edpr_logo.png b/humanPose/edpr_logo.png new file mode 100644 index 0000000..8420d96 Binary files /dev/null and b/humanPose/edpr_logo.png differ diff --git a/humanPose/hpe_msgs/NC_humanPose.h b/humanPose/hpe_msgs/NC_humanPose.h new file mode 100644 index 0000000..b756391 --- /dev/null +++ b/humanPose/hpe_msgs/NC_humanPose.h @@ -0,0 +1,199 @@ +/* + * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) + * SPDX-License-Identifier: BSD-3-Clause + */ + +// This is an automatically generated file. + +// Generated from the following "NC_humanPose" msg definition: +// float64 timestamp +// float64[] pose +// float64[] velocity// Instances of this class can be read and written with YARP ports, +// using a ROS-compatible format. + +#ifndef YARP_ROSMSG_NC_humanPose_h +#define YARP_ROSMSG_NC_humanPose_h + +#include +#include +#include +#include +#include + +namespace yarp { +namespace rosmsg { + +class NC_humanPose : public yarp::os::idl::WirePortable +{ +public: + yarp::conf::float64_t timestamp; + std::vector pose; + std::vector velocity; + + NC_humanPose() : + timestamp(0.0), + pose(), + velocity() + { + } + + void clear() + { + // *** timestamp *** + timestamp = 0.0; + + // *** pose *** + pose.clear(); + + // *** velocity *** + velocity.clear(); + } + + bool readBare(yarp::os::ConnectionReader& connection) override + { + // *** timestamp *** + timestamp = connection.expectFloat64(); + + // *** pose *** + int len = connection.expectInt32(); + pose.resize(len); + if (len > 0 && !connection.expectBlock((char*)&pose[0], sizeof(yarp::conf::float64_t)*len)) { + return false; + } + + // *** velocity *** + len = connection.expectInt32(); + velocity.resize(len); + if (len > 0 && !connection.expectBlock((char*)&velocity[0], sizeof(yarp::conf::float64_t)*len)) { + return false; + } + + return !connection.isError(); + } + + bool readBottle(yarp::os::ConnectionReader& connection) override + { + connection.convertTextMode(); + yarp::os::idl::WireReader reader(connection); + if (!reader.readListHeader(3)) { + return false; + } + + // *** timestamp *** + timestamp = reader.expectFloat64(); + + // *** pose *** + if (connection.expectInt32() != (BOTTLE_TAG_LIST|BOTTLE_TAG_FLOAT64)) { + return false; + } + int len = connection.expectInt32(); + pose.resize(len); + for (int i=0; i0) { + connection.appendExternalBlock((char*)&pose[0], sizeof(yarp::conf::float64_t)*pose.size()); + } + + // *** velocity *** + connection.appendInt32(velocity.size()); + if (velocity.size()>0) { + connection.appendExternalBlock((char*)&velocity[0], sizeof(yarp::conf::float64_t)*velocity.size()); + } + + return !connection.isError(); + } + + bool writeBottle(yarp::os::ConnectionWriter& connection) const override + { + connection.appendInt32(BOTTLE_TAG_LIST); + connection.appendInt32(3); + + // *** timestamp *** + connection.appendInt32(BOTTLE_TAG_FLOAT64); + connection.appendFloat64(timestamp); + + // *** pose *** + connection.appendInt32(BOTTLE_TAG_LIST|BOTTLE_TAG_FLOAT64); + connection.appendInt32(pose.size()); + for (size_t i=0; i rosStyle; + typedef yarp::os::idl::BottleStyle bottleStyle; + + // The name for this message, ROS will need this + static constexpr const char* typeName = "NC_humanPose"; + + // The checksum for this message, ROS will need this + static constexpr const char* typeChecksum = "f60fac524517d870dbaac768aeafa188"; + + // The source text for this message, ROS will need this + static constexpr const char* typeText = "\ +float64 timestamp\n\ +float64[] pose\n\ +float64[] velocity\n\ +"; + + yarp::os::Type getType() const override + { + yarp::os::Type typ = yarp::os::Type::byName(typeName, typeName); + typ.addProperty("md5sum", yarp::os::Value(typeChecksum)); + typ.addProperty("message_definition", yarp::os::Value(typeText)); + return typ; + } +}; + +} // namespace rosmsg +} // namespace yarp + +#endif // YARP_ROSMSG_NC_humanPose_h diff --git a/humanPose/hpe_msgs/NC_humanPose.msg b/humanPose/hpe_msgs/NC_humanPose.msg new file mode 100644 index 0000000..5161699 --- /dev/null +++ b/humanPose/hpe_msgs/NC_humanPose.msg @@ -0,0 +1,3 @@ +float64 timestamp +float64[] pose +float64[] velocity \ No newline at end of file diff --git a/humanPose/kill_hpe.sh b/humanPose/kill_hpe.sh new file mode 100644 index 0000000..0cb1fa4 --- /dev/null +++ b/humanPose/kill_hpe.sh @@ -0,0 +1,11 @@ +#!/bin/bash +echo "Killing all HPE tasks" +echo "=======================" +killall python3 +echo "Killed MoveEnet" +killall edpr-hpe +echo "Killed EDPR HPE app" +killall atis-bridge-sdk +echo "ATIS-bridge killed" +killall yarpserver +echo "YARP server killed" diff --git a/humanPose/run_hpe.sh b/humanPose/run_hpe.sh new file mode 100644 index 0000000..eb8e6ef --- /dev/null +++ b/humanPose/run_hpe.sh @@ -0,0 +1,33 @@ +#!/bin/bash + +while getopts 'dm:' OPTION; do + case "$OPTION" in + d) + echo "ROS_MASTER_URI set to default value" + export ROS_MASTER_URI=http://127.0.0.1:11311 + echo "ROS_MASTER_URI="$ROS_MASTER_URI + ;; + m) + echo "ROS_MASTER_URI set manually" + export ROS_MASTER_URI="$OPTARG" + echo "ROS_MASTER_URI="$ROS_MASTER_URI + ;; + ?) + echo "script usage: /run_hpe.sh [-d (default)] or [-m value (manual)]" >&2 + exit 1 + ;; + esac +done + +if [ "$#" -eq "0" ] + then + echo "ROS_MASTER_URI not set" + exit 1 +fi + +echo "Run YARP server connected to ROS" +yarpserver --ros & +echo "Run ATIS-bridge" +atis-bridge-sdk --s 60 --filter 0.01 & +echo "Run EDPR-HPE application" +edpr-hpe