diff --git a/.clang-format b/.clang-format new file mode 100644 index 000000000..33bf2a3b9 --- /dev/null +++ b/.clang-format @@ -0,0 +1,137 @@ +--- +Language: Cpp +# BasedOnStyle: LLVM +AccessModifierOffset: -2 +AlignAfterOpenBracket: Align +AlignConsecutiveMacros: false +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false +AlignEscapedNewlines: Right +AlignOperands: true +AlignTrailingComments: true +AllowAllArgumentsOnNextLine: true +AllowAllConstructorInitializersOnNextLine: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: Never +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: All +AllowShortLambdasOnASingleLine: All +AllowShortIfStatementsOnASingleLine: Never +AllowShortLoopsOnASingleLine: false +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: false +AlwaysBreakTemplateDeclarations: MultiLine +BinPackArguments: true +BinPackParameters: true +BraceWrapping: + AfterCaseLabel: false + AfterClass: false + AfterControlStatement: false + AfterEnum: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + AfterExternBlock: false + BeforeCatch: false + BeforeElse: false + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakBeforeBinaryOperators: None +BreakBeforeBraces: Attach +BreakBeforeInheritanceComma: false +BreakInheritanceList: BeforeColon +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: false +BreakConstructorInitializers: BeforeColon +BreakAfterJavaFieldAnnotations: false +BreakStringLiterals: true +ColumnLimit: 80 +CommentPragmas: '^ IWYU pragma:' +CompactNamespaces: false +ConstructorInitializerAllOnOneLineOrOnePerLine: false +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DeriveLineEnding: true +DerivePointerAlignment: false +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: true +ForEachMacros: + - foreach + - Q_FOREACH + - BOOST_FOREACH +IncludeBlocks: Preserve +IncludeCategories: + - Regex: '^"(llvm|llvm-c|clang|clang-c)/' + Priority: 2 + SortPriority: 0 + - Regex: '^(<|"(gtest|gmock|isl|json)/)' + Priority: 3 + SortPriority: 0 + - Regex: '.*' + Priority: 1 + SortPriority: 0 +IncludeIsMainRegex: '(Test)?$' +IncludeIsMainSourceRegex: '' +IndentCaseLabels: false +IndentGotoLabels: true +IndentPPDirectives: None +IndentWidth: 2 +IndentWrappedFunctionNames: false +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: true +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBinPackProtocolList: Auto +ObjCBlockIndentWidth: 2 +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: true +PenaltyBreakAssignment: 2 +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakString: 1000 +PenaltyBreakTemplateDeclaration: 10 +PenaltyExcessCharacter: 1000000 +PenaltyReturnTypeOnItsOwnLine: 60 +PointerAlignment: Right +ReflowComments: true +SortIncludes: true +SortUsingDeclarations: true +SpaceAfterCStyleCast: false +SpaceAfterLogicalNot: false +SpaceAfterTemplateKeyword: true +SpaceBeforeAssignmentOperators: true +SpaceBeforeCpp11BracedList: false +SpaceBeforeCtorInitializerColon: true +SpaceBeforeInheritanceColon: true +SpaceBeforeParens: ControlStatements +SpaceBeforeRangeBasedForLoopColon: true +SpaceInEmptyBlock: false +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 1 +SpacesInAngles: false +SpacesInConditionalStatement: false +SpacesInContainerLiterals: true +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +SpaceBeforeSquareBrackets: false +Standard: Latest +StatementMacros: + - Q_UNUSED + - QT_REQUIRE_VERSION +TabWidth: 8 +UseCRLF: false +UseTab: Never +... + diff --git a/.github/workflows/ROS_build.yml b/.github/workflows/ROS_build.yml deleted file mode 100644 index 4e0a6ed48..000000000 --- a/.github/workflows/ROS_build.yml +++ /dev/null @@ -1,56 +0,0 @@ -name: ROS melodic CI -on: - pull_request: - branches: - - development - -defaults: - run: - shell: bash - -jobs: - build-and-test-run: - runs-on: ubuntu-18.04 - - steps: - - uses: actions/checkout@v1 - - name: Install ROS and workspace dependencies - run: | - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' && - sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 && - sudo apt-get update && - sudo apt-get -qq update -y && - ( sudo apt-get -qq install build-essential openssh-client ros-melodic-desktop-full ros-melodic-catkin python-catkin-tools python-openpyxl python3-catkin-pkg python3-rosdep ros-melodic-diagnostics ros-melodic-geometry protobuf-compiler ros-melodic-rosbridge-server ros-melodic-message-to-tf ros-melodic-geographic-msgs ros-melodic-move-base ros-melodic-move-base-msgs -y || - sudo apt-get -qq install build-essential openssh-client ros-melodic-desktop-full ros-melodic-catkin python-catkin-tools python-catkin-pkg python-rosdep ros-melodic-diagnostics ros-melodic-geometry protobuf-compiler ros-melodic-rosbridge-server ros-melodic-message-to-tf ros-melodic-geographic-msgs ros-melodic-move-base ros-melodic-move-base-msgs -y ; ) && - sudo rosdep init && - rosdep update`; - rosdep install --from-paths --reinstall --ignore-packages-from-source --default-yes --verbose .`; - - - name: Setup catkin workspace - run: | - mkdir -p ~/catkin_ws/src - cd ~/catkin_ws - source /opt/ros/melodic/setup.bash >> ~/.bashrc - source ~/.bashrc - catkin build - - - name: Clone repositories - run: | - cd ~/catkin_ws/src - cp -r $GITHUB_WORKSPACE . - git clone https://github.com/vortexNTNU/Vortex-Simulator - - - name: Build packages - run: | - cd ~/catkin_ws/ - source devel/setup.bash && catkin build - source devel/setup.bash - - - name: Run Vortex-AUV with Vortex-Simulator - run: | - cd ~/catkin_ws/ - source /opt/ros/melodic/setup.bash - source ~/catkin_ws/devel/setup.bash - cd ~/catkin_ws/src/Vortex-Simulator/launch_scripts_bash - chmod u+x+r ./sim_launch_fsm.sh - ./sim_launch_fsm.sh --gui 0 --camerafront 0 --sleep 25 diff --git a/.github/workflows/black-format-check.yml b/.github/workflows/black-format-check.yml new file mode 100644 index 000000000..a1c7b1763 --- /dev/null +++ b/.github/workflows/black-format-check.yml @@ -0,0 +1,19 @@ +name: black-format check +on: [push] +jobs: + linter_name: + name: runner / black formatter + runs-on: ubuntu-20.04 + steps: + - uses: actions/checkout@v2 + + - name: Setup Python + uses: actions/setup-python@v2 + with: + python-version: 3.8 + + - name: Install deps + run: pip install black + + - name: Run Python black formatting check + run: python3 -m black . --check --diff diff --git a/.github/workflows/clang-format-check.yml b/.github/workflows/clang-format-check.yml new file mode 100644 index 000000000..a2137cb67 --- /dev/null +++ b/.github/workflows/clang-format-check.yml @@ -0,0 +1,16 @@ +name: clang-format check + +on: [ push ] + +jobs: + formatting-check: + name: C/C++ LLVM formatting check + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - name: Run clang-format style check for C/C++ packages + uses: jidicula/clang-format-action@v4.5.0 + with: + clang-format-version: '12' + check-path: '.' + fallback-style: 'llvm' # for when the .clang-format file does not exist diff --git a/.github/workflows/docker-publish.yml b/.github/workflows/docker-publish.yml new file mode 100644 index 000000000..89f4204f7 --- /dev/null +++ b/.github/workflows/docker-publish.yml @@ -0,0 +1,41 @@ +name: Publish Docker image to ghcr + +on: + push: + branches: [ development ] + +env: + REGISTRY: ghcr.io + IMAGE_NAME: ${{ github.repository }} + +jobs: + build-and-push-image: + runs-on: ubuntu-latest + permissions: + contents: read + packages: write + + steps: + - name: Checkout repository + uses: actions/checkout@v2 + + - name: Log in to the Container registry + uses: docker/login-action@f054a8b539a109f9f41c372932f1ae047eff08c9 + with: + registry: ${{ env.REGISTRY }} + username: ${{ github.actor }} + password: ${{ secrets.GITHUB_TOKEN }} + + - name: Extract metadata (tags, labels) for Docker + id: meta + uses: docker/metadata-action@98669ae865ea3cffbcbaa878cf57c20bbf1c6c38 + with: + images: ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }} + + - name: Build and push Docker image + uses: docker/build-push-action@ad44023a93711e3deb337508980b4b5e9bcdc5dc + with: + context: . + push: true + tags: ${{ steps.meta.outputs.tags }} + labels: ${{ steps.meta.outputs.labels }} diff --git a/.github/workflows/ros-build-check.yml b/.github/workflows/ros-build-check.yml new file mode 100644 index 000000000..591ce4ab5 --- /dev/null +++ b/.github/workflows/ros-build-check.yml @@ -0,0 +1,24 @@ +name: ROS build check + +on: + push: + branches: [ development ] + pull_request: + branches: [ development ] + +defaults: + run: + shell: bash + +jobs: + build-and-test-run: + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v1 + + - name: Build the Docker image + run: docker build . --file Dockerfile --tag auv:ci-build + # Note that the Dockerfile contains a RUN that builds the catkin workspace. Running will however not work due to a cross-dep with another repo + # For this reason, the image is also not uploaded to the registry diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 000000000..5da99c81a --- /dev/null +++ b/Dockerfile @@ -0,0 +1,60 @@ +FROM ros:noetic + +ARG distro=noetic +ENV DEBIAN_FRONTEND=noninteractive +SHELL ["/bin/bash", "-c"] + +# ROS package dependencies +RUN apt update && \ + apt install -y --no-install-recommends \ + apt-utils \ + net-tools \ + tcpdump \ + nano \ + git \ + ros-$distro-roslint \ + ros-$distro-move-base-msgs \ + ros-$distro-tf \ + ros-$distro-tf2 \ + ros-$distro-eigen-conversions \ + ros-$distro-joy \ + ros-$distro-tf2-geometry-msgs \ + ros-$distro-pcl-ros \ + ros-$distro-rviz \ + ros-$distro-rosbridge-server \ + ros-$distro-message-to-tf \ + ros-$distro-geographic-msgs \ + ros-$distro-move-base \ + ros-$distro-smach-ros \ + ros-$distro-tf-conversions \ + python3-osrf-pycommon \ + python3-openpyxl \ + python3-yaml \ + python3-pip \ + python3-wheel \ + python3-catkin-tools \ + python3-vcstool \ + python3-pandas \ + python3-scipy \ + libgeographic-dev \ + libeigen3-dev \ + libglfw3-dev \ + libglew-dev \ + libjsoncpp-dev \ + libtclap-dev \ + protobuf-compiler + +RUN pip install \ + rospkg \ + pyquaternion \ + quadprog \ + sklearn \ + enum34 + +COPY . /vortex_ws/src/Vortex-AUV +RUN cd /vortex_ws/src && git clone https://github.com/vortexntnu/robot_localization +RUN source /opt/ros/$distro/setup.bash && cd /vortex_ws && catkin build + +COPY ./entrypoint.sh / + +ENTRYPOINT ["/entrypoint.sh"] \ No newline at end of file diff --git a/Dockerfile.dev b/Dockerfile.dev new file mode 100644 index 000000000..693551702 --- /dev/null +++ b/Dockerfile.dev @@ -0,0 +1,66 @@ +FROM ros:noetic + +ARG distro=noetic +ENV DEBIAN_FRONTEND=noninteractive + +# Create vortex user +RUN useradd -ms /bin/bash \ + --home /home/vortex vortex +RUN echo "vortex:vortex" | chpasswd +RUN usermod -aG sudo vortex + +# ROS package dependencies +RUN apt update && \ + apt install -y --no-install-recommends \ + apt-utils \ + net-tools \ + tcpdump \ + nano \ + git \ + ros-$distro-roslint \ + ros-$distro-move-base-msgs \ + ros-$distro-tf \ + ros-$distro-tf2 \ + ros-$distro-eigen-conversions \ + ros-$distro-joy \ + ros-$distro-tf2-geometry-msgs \ + ros-$distro-pcl-ros \ + ros-$distro-rviz \ + ros-$distro-rosbridge-server \ + ros-$distro-message-to-tf \ + ros-$distro-geographic-msgs \ + ros-$distro-move-base \ + ros-$distro-smach-ros \ + ros-$distro-tf-conversions \ + python3-osrf-pycommon \ + python3-openpyxl \ + python3-yaml \ + python3-pip \ + python3-wheel \ + python3-catkin-tools \ + python3-vcstool \ + python3-pandas \ + python3-scipy \ + libgeographic-dev \ + libeigen3-dev \ + libglfw3-dev \ + libglew-dev \ + libjsoncpp-dev \ + libtclap-dev \ + protobuf-compiler + +RUN pip install \ + rospkg \ + pyquaternion \ + quadprog \ + sklearn \ + enum34 + +RUN echo "source /opt/ros/noetic/setup.bash" >> /home/vortex/.bashrc +RUN echo "source /home/vortex/auv_ws/devel/setup.bash" >> /home/vortex/.bashrc +RUN touch /home/vortex/.bash_aliases && echo "alias srcdev='source devel/setup.bash'" >> /home/vortex/.bash_aliases + +RUN mkdir -p /home/vortex/auv_ws/src +RUN chown -R vortex /home/vortex/auv_ws + +CMD ["/bin/bash"] \ No newline at end of file diff --git a/docker-compose.yml b/docker-compose.yml new file mode 100644 index 000000000..2923ae1c3 --- /dev/null +++ b/docker-compose.yml @@ -0,0 +1,27 @@ +version: '3.8' +services: + + auv: + container_name: auv + build: . + image: auv:latest + stdin_open: true + tty: true + network_mode: host + + auv-dev: + container_name: auv-dev + build: + context: . + dockerfile: ./Dockerfile.dev + image: auv:dev + user: vortex + privileged: true + volumes: + - "../..:/home/vortex/auv_ws/src" + - "/dev:/dev" + environment: + DISPLAY: :0 + stdin_open: true + tty: true + network_mode: host diff --git a/entrypoint.sh b/entrypoint.sh new file mode 100755 index 000000000..adba2f794 --- /dev/null +++ b/entrypoint.sh @@ -0,0 +1,22 @@ +#!/bin/bash + +drone="beluga" +type="simulator" + +while getopts ":d:t" opt; do + case $opt in + d) + drone=$OPTARG + ;; + t) + type=$OPTARG + ;; + \?) + echo "Invalid option: -$OPTARG" >&2 + exit 1 + ;; + esac +done + +source /vortex_ws/devel/setup.bash +roslaunch auv_setup $drone.launch type:=$type \ No newline at end of file diff --git a/manipulators/gripper_interface/scripts/gripper_interface_node.py b/manipulators/gripper_interface/scripts/gripper_interface_node.py index 3f3b5e801..0ea12b97c 100755 --- a/manipulators/gripper_interface/scripts/gripper_interface_node.py +++ b/manipulators/gripper_interface/scripts/gripper_interface_node.py @@ -16,14 +16,17 @@ active = 1 inactive = 0 -class GripperInterfaceNode(): + +class GripperInterfaceNode: def __init__(self): - rospy.init_node('gripper_interface') - self.joystick_sub = rospy.Subscriber('/mission/joystick_data', Joy, self.callback, queue_size=1) + rospy.init_node("gripper_interface") + self.joystick_sub = rospy.Subscriber( + "/mission/joystick_data", Joy, self.callback, queue_size=1 + ) self.gripper_state = inactive - self.cooldown_period = 0.4 # Seconds + self.cooldown_period = 0.4 # Seconds self.last_press = datetime.now() # GPIO setup @@ -33,9 +36,9 @@ def __init__(self): GPIO.output(self.gripper_gpio_pin, GPIO.LOW) def callback(self, joy_msg): - + Dpad = joy_msg.axes[7] - if Dpad != 0: # only handle non-zero messages, since the joy topic is spammed + if Dpad != 0: # only handle non-zero messages, since the joy topic is spammed time_delta = datetime.now() - self.last_press if time_delta.total_seconds() > self.cooldown_period: @@ -50,16 +53,17 @@ def callback(self, joy_msg): elif Dpad == off and self.gripper_state == active: GPIO.output(self.gripper_gpio_pin, GPIO.LOW) rospy.loginfo("Gripper deactivated!") - + self.last_press = datetime.now() self.gripper_state = inactive def shutdown(self): GPIO.output(self.gripper_gpio_pin, GPIO.LOW) -if __name__ == '__main__': + +if __name__ == "__main__": node = GripperInterfaceNode() - + while not rospy.is_shutdown(): rospy.spin() diff --git a/manipulators/lights_interface/scripts/lights_interface_node.py b/manipulators/lights_interface/scripts/lights_interface_node.py index 6dd04ef23..1c15f3892 100755 --- a/manipulators/lights_interface/scripts/lights_interface_node.py +++ b/manipulators/lights_interface/scripts/lights_interface_node.py @@ -14,24 +14,29 @@ PWM_LOW = 1100 PWM_HIGH = 1900 + class LightsInterfaceNode: def __init__(self): - rospy.init_node('lights_interface') + rospy.init_node("lights_interface") - self.js_sub = rospy.Subscriber('/joystick/joy', Joy, self.callback, queue_size=1) - self.pwm_pub = rospy.Publisher('/pwm', Pwm, queue_size=1) + self.js_sub = rospy.Subscriber( + "/joystick/joy", Joy, self.callback, queue_size=1 + ) + self.pwm_pub = rospy.Publisher("/pwm", Pwm, queue_size=1) self.pwm_pin = 8 self.light_state = inactive - self.cooldown_period = 0.4 # Seconds + self.cooldown_period = 0.4 # Seconds self.last_press = datetime.now() def callback(self, joy_msg): - + button = joy_msg.buttons[7] - - if button == pressed: # only handle non-zero messages, since the joy topic is spammed + + if ( + button == pressed + ): # only handle non-zero messages, since the joy topic is spammed time_delta = datetime.now() - self.last_press if time_delta.total_seconds() > self.cooldown_period: @@ -48,16 +53,17 @@ def callback(self, joy_msg): self.light_state = inactive def publish_pwm_msg(self, us): - msg = Pwm() - msg.pins = [self.pwm_pin] - msg.positive_width_us = [us] + msg = Pwm() + msg.pins = [self.pwm_pin] + msg.positive_width_us = [us] - self.pwm_pub.publish(msg) + self.pwm_pub.publish(msg) # If PWM set is not sticky, have a spin function that writes -if __name__ == '__main__': + +if __name__ == "__main__": node = LightsInterfaceNode() - + while not rospy.is_shutdown(): rospy.spin() diff --git a/manipulators/torpedo/scripts/torpedo_launch_service.py b/manipulators/torpedo/scripts/torpedo_launch_service.py index 315d5b801..48237de75 100755 --- a/manipulators/torpedo/scripts/torpedo_launch_service.py +++ b/manipulators/torpedo/scripts/torpedo_launch_service.py @@ -7,23 +7,24 @@ # How long to wait after a pin has been set to high -FIRING_DURATION = 3 # in seconds +FIRING_DURATION = 3 # in seconds # the two torpedos pin assignments. These have to be specified in a config file -class TorpedoLaunch(): - +class TorpedoLaunch: def __init__(self): -######## Definig the node ######## - rospy.init_node('torpedo_node') + ######## Definig the node ######## + rospy.init_node("torpedo_node") -######## Defining the Service ######## - self.torpedo_service = rospy.Service('manipulator/torpedo_launch', LaunchTorpedo, self.execute) + ######## Defining the Service ######## + self.torpedo_service = rospy.Service( + "manipulator/torpedo_launch", LaunchTorpedo, self.execute + ) -######## GPIO Setup ######## + ######## GPIO Setup ######## self.mode = GPIO.LOW - self.torpedo_gpio_pin = rospy.get_param('/torpedo/gpio_pin') + self.torpedo_gpio_pin = rospy.get_param("/torpedo/gpio_pin") GPIO.setmode(GPIO.BOARD) GPIO.setup(self.torpedo_gpio_pin, GPIO.OUT, initial=self.mode) @@ -42,7 +43,6 @@ def execute(self, req): return res - if __name__ == "__main__": node = TorpedoLaunch() diff --git a/mission/anomaly_detection/scripts/battery_simulator.py b/mission/anomaly_detection/scripts/battery_simulator.py index 7df0a4b28..98be8bc98 100755 --- a/mission/anomaly_detection/scripts/battery_simulator.py +++ b/mission/anomaly_detection/scripts/battery_simulator.py @@ -12,18 +12,24 @@ from anomaly_detection.cfg import BatterySimulatorConfig import thread -class BatterySimulator(): +class BatterySimulator: def __init__(self): - + rospy.init_node("battery_simulator") # get parameters - self.rate = rospy.get_param('~rate', 1) - self.battery_runtime = rospy.get_param("~battery_runtime", 400) # seconds - self.initial_battery_level = rospy.get_param('~initial_battery_level', 100) # percentage - self.error_battery_level = rospy.get_param('~error_battery_level', 20) # percentage - self.warn_battery_level = rospy.get_param('~warn_battery_level', 50) # percentage + self.rate = rospy.get_param("~rate", 1) + self.battery_runtime = rospy.get_param("~battery_runtime", 400) # seconds + self.initial_battery_level = rospy.get_param( + "~initial_battery_level", 100 + ) # percentage + self.error_battery_level = rospy.get_param( + "~error_battery_level", 20 + ) # percentage + self.warn_battery_level = rospy.get_param( + "~warn_battery_level", 50 + ) # percentage # set parameters ros_rate = rospy.Rate(self.rate) @@ -31,21 +37,31 @@ def __init__(self): self.new_battery_level = self.initial_battery_level # the step size to decrease battery level - self.battery_step = float(self.initial_battery_level) / self.rate / self.battery_runtime + self.battery_step = ( + float(self.initial_battery_level) / self.rate / self.battery_runtime + ) # mutually exclusive flag to enforce mutual exclusion concurrency control policy self.mutex = thread.allocate_lock() # topics - self.battery_level_pub = rospy.Publisher("/auv/battery_level", Float32, queue_size=1) - self.diag_pub = rospy.Publisher("/auv/diagnostics", DiagnosticArray, queue_size=1) + self.battery_level_pub = rospy.Publisher( + "/auv/battery_level", Float32, queue_size=1 + ) + self.diag_pub = rospy.Publisher( + "/auv/diagnostics", DiagnosticArray, queue_size=1 + ) # services # manually set battery level - rospy.Service('~set_battery_level', SetBatteryLevel, self.SetBatteryLevelHandler) + rospy.Service( + "~set_battery_level", SetBatteryLevel, self.SetBatteryLevelHandler + ) # create a dynamic reconfigure server and set a callback function - self.battery_dyn_reconfigure = Server(BatterySimulatorConfig, self.dynamic_reconfigure_callback) + self.battery_dyn_reconfigure = Server( + BatterySimulatorConfig, self.dynamic_reconfigure_callback + ) while not rospy.is_shutdown(): @@ -65,44 +81,47 @@ def __init__(self): status.level = DiagnosticStatus.OK # add the current raw battery level to the diagnostics message - status.values.append(KeyValue("Battery Level", str(self.current_battery_level))) + status.values.append( + KeyValue("Battery Level", str(self.current_battery_level)) + ) # build the diagnostics array message msg = DiagnosticArray() msg.header.stamp = rospy.Time.now() msg.status.append(status) - + # publish self.diag_pub.publish(msg) self.battery_level_pub.publish(self.current_battery_level) # adjust current battery level - self.current_battery_level = max(0, self.current_battery_level - self.battery_step) + self.current_battery_level = max( + 0, self.current_battery_level - self.battery_step + ) ros_rate.sleep() def dynamic_reconfigure_callback(self, config, level): - if self.battery_runtime != config['battery_runtime']: - self.battery_runtime = config['battery_runtime'] + if self.battery_runtime != config["battery_runtime"]: + self.battery_runtime = config["battery_runtime"] self.battery_step = 100.0 / self.rate / self.battery_runtime - if self.new_battery_level != config['new_battery_level']: - self.new_battery_level = config['new_battery_level'] - self.mutex.acquire() # set lock + if self.new_battery_level != config["new_battery_level"]: + self.new_battery_level = config["new_battery_level"] + self.mutex.acquire() # set lock self.current_battery_level = self.new_battery_level - self.mutex.release() # release lock + self.mutex.release() # release lock return config def SetBatteryLevelHandler(self, req): - self.mutex.acquire() # set lock + self.mutex.acquire() # set lock self.current_battery_level = req.value - self.mutex.release() # release lock + self.mutex.release() # release lock return SetBatteryLevelResponse() +if __name__ == "__main__": -if __name__ == '__main__': - - BatterySimulator() \ No newline at end of file + BatterySimulator() diff --git a/mission/finite_state_machine/scripts/buoy.py b/mission/finite_state_machine/scripts/buoy.py index 2e041a7a2..32755b5aa 100644 --- a/mission/finite_state_machine/scripts/buoy.py +++ b/mission/finite_state_machine/scripts/buoy.py @@ -7,39 +7,53 @@ import actionlib from actionlib_msgs.msg import GoalStatus from move_base_msgs.msg import MoveBaseAction, MoveBaseActionGoal, MoveBaseGoal -from vortex_msgs.msg import VtfPathFollowingAction, VtfPathFollowingGoal, SetVelocityGoal, SetVelocityAction, DpSetpoint +from vortex_msgs.msg import ( + VtfPathFollowingAction, + VtfPathFollowingGoal, + SetVelocityGoal, + SetVelocityAction, + DpSetpoint, +) from tf.transformations import quaternion_from_euler from fsm_helper import dp_move, get_pose_in_front, rotate_certain_angle from vortex_msgs.srv import ControlMode + class BuoySearch(smach.State): def __init__(self): - smach.State.__init__(self, outcomes=['preempted', 'succeeded', 'aborted']) - self.landmarks_client = rospy.ServiceProxy('send_positions',request_position) - rospy.wait_for_service('send_positions') + smach.State.__init__(self, outcomes=["preempted", "succeeded", "aborted"]) + self.landmarks_client = rospy.ServiceProxy("send_positions", request_position) + rospy.wait_for_service("send_positions") self.object = self.landmarks_client("buoy").object - self.state_pub = rospy.Publisher('/fsm/state',String,queue_size=1) + self.state_pub = rospy.Publisher("/fsm/state", String, queue_size=1) vtf_action_server = "/controllers/vtf_action_server" - self.vtf_client = actionlib.SimpleActionClient(vtf_action_server, VtfPathFollowingAction) - + self.vtf_client = actionlib.SimpleActionClient( + vtf_action_server, VtfPathFollowingAction + ) def execute(self, userdata): - return 'succeeded' + return "succeeded" class BuoyConverge(smach.State): def __init__(self): - smach.State.__init__(self, outcomes=['preempted', 'succeeded', 'aborted'],output_keys=['buoy_converge_output']) - - self.landmarks_client = rospy.ServiceProxy('send_positions',request_position) - rospy.wait_for_service('send_positions') + smach.State.__init__( + self, + outcomes=["preempted", "succeeded", "aborted"], + output_keys=["buoy_converge_output"], + ) + + self.landmarks_client = rospy.ServiceProxy("send_positions", request_position) + rospy.wait_for_service("send_positions") self.object = self.landmarks_client("buoy").object vtf_action_server = "/controllers/vtf_action_server" - self.vtf_client = actionlib.SimpleActionClient(vtf_action_server, VtfPathFollowingAction) + self.vtf_client = actionlib.SimpleActionClient( + vtf_action_server, VtfPathFollowingAction + ) self.dp_pub = rospy.Publisher("controllers/dp_data", DpSetpoint) @@ -47,7 +61,7 @@ def execute(self, userdata): goal = VtfPathFollowingGoal() self.object = self.landmarks_client("buoy").object - goal_pose = get_pose_in_front(self.object.objectPose.pose, 0.5) + goal_pose = get_pose_in_front(self.object.objectPose.pose, 0.5) print("get_pose_in_front returned:") print(goal_pose) goal.waypoints = [goal_pose.position] @@ -58,52 +72,72 @@ def execute(self, userdata): self.vtf_client.send_goal(goal) rate = rospy.Rate(1) rate.sleep() - while not rospy.is_shutdown() \ - and not self.vtf_client.simple_state == actionlib.simple_action_client.SimpleGoalState.DONE: + while ( + not rospy.is_shutdown() + and not self.vtf_client.simple_state + == actionlib.simple_action_client.SimpleGoalState.DONE + ): self.object = self.landmarks_client("buoy").object goal.waypoints = [self.object.objectPose.pose.position] - print("BUOY POSITION DETECTED: "+ \ - str(goal.waypoints[0].x) + ", "+ str(goal.waypoints[0].y)+ ", "+ str(goal.waypoints[0].z)) - goal.waypoints[0] = get_pose_in_front(self.object.objectPose.pose, 0.5).position + print( + "BUOY POSITION DETECTED: " + + str(goal.waypoints[0].x) + + ", " + + str(goal.waypoints[0].y) + + ", " + + str(goal.waypoints[0].z) + ) + goal.waypoints[0] = get_pose_in_front( + self.object.objectPose.pose, 0.5 + ).position self.vtf_client.send_goal(goal) rate.sleep() if self.object.estimateFucked: self.vtf_client.cancel_all_goals() - return 'aborted' + return "aborted" self.vtf_client.cancel_all_goals() dp_goal = DpSetpoint() - dp_goal.control_mode = 7 # POSE_HOLD + dp_goal.control_mode = 7 # POSE_HOLD dp_goal.setpoint = get_pose_in_front(self.object.objectPose.pose, 0.5) self.dp_pub.publish(dp_goal) - while not rospy.is_shutdown()\ - and not self.object.estimateConverged: + while not rospy.is_shutdown() and not self.object.estimateConverged: self.object = self.landmarks_client("buoy").object if self.object.estimateFucked: - dp_goal.control_mode = 0 # OPEN_LOOP + dp_goal.control_mode = 0 # OPEN_LOOP self.dp_pub.publish(dp_goal) - return 'aborted' + return "aborted" rate.sleep() - dp_goal.control_mode = 0 # OPEN_LOOP + dp_goal.control_mode = 0 # OPEN_LOOP self.dp_pub.publish(dp_goal) self.object = self.landmarks_client("buoy").object - userdata.buoy_converge_output=self.object - print("BUOY POSITION ESTIMATE CONVERGED AT: " + str(self.object.objectPose.pose.position.x) + "; " \ - + str(self.object.objectPose.pose.position.y) + "; " \ - + str(self.object.objectPose.pose.position.z)) - return 'succeeded' + userdata.buoy_converge_output = self.object + print( + "BUOY POSITION ESTIMATE CONVERGED AT: " + + str(self.object.objectPose.pose.position.x) + + "; " + + str(self.object.objectPose.pose.position.y) + + "; " + + str(self.object.objectPose.pose.position.z) + ) + return "succeeded" + class BuoyExecute(smach.State): def __init__(self): - smach.State.__init__(self, outcomes=['preempted', 'succeeded', 'aborted'],input_keys=['buoy']) - + smach.State.__init__( + self, outcomes=["preempted", "succeeded", "aborted"], input_keys=["buoy"] + ) + vtf_action_server = "/controllers/vtf_action_server" - self.vtf_client = actionlib.SimpleActionClient(vtf_action_server, VtfPathFollowingAction) + self.vtf_client = actionlib.SimpleActionClient( + vtf_action_server, VtfPathFollowingAction + ) def execute(self, userdata): goal = VtfPathFollowingGoal() - goal_pose = get_pose_in_front(userdata.buoy.objectPose.pose,-0.5 ) - goal.waypoints =[goal_pose.position] + goal_pose = get_pose_in_front(userdata.buoy.objectPose.pose, -0.5) + goal.waypoints = [goal_pose.position] goal.forward_speed = rospy.get_param("/fsm/medium_speed") goal.heading = "path_dependent_heading" @@ -112,12 +146,11 @@ def execute(self, userdata): rate = rospy.Rate(1) rate.sleep() while not rospy.is_shutdown(): - if self.vtf_client.simple_state == actionlib.simple_action_client.SimpleGoalState.DONE: + if ( + self.vtf_client.simple_state + == actionlib.simple_action_client.SimpleGoalState.DONE + ): break rate.sleep() - return 'succeeded' - - - - \ No newline at end of file + return "succeeded" diff --git a/mission/finite_state_machine/scripts/four_corner_mission.py b/mission/finite_state_machine/scripts/four_corner_mission.py index d2280feb9..53a10c4e0 100755 --- a/mission/finite_state_machine/scripts/four_corner_mission.py +++ b/mission/finite_state_machine/scripts/four_corner_mission.py @@ -6,37 +6,36 @@ from smach_ros import SimpleActionState from vortex_msgs.msg import MoveAction + def main(): """ A simple state machine which moves the AUV in a rectangle pattern using dp_move() and ls_move. This state machine is inteded for testing/data collection. """ - rospy.init_node('four_corner_sm') + rospy.init_node("four_corner_sm") - move_action_server = 'guidance/move' + move_action_server = "guidance/move" rectangle_length = 4 four_corner_sm = Sequence( - outcomes = ['succeeded','aborted','preempted'], - connector_outcome = 'succeeded') + outcomes=["succeeded", "aborted", "preempted"], connector_outcome="succeeded" + ) with four_corner_sm: - Sequence.add('1ST_CORNER', dp_move(0,0)) - Sequence.add('2ND_CORNER', los_move(rectangle_length,0)) - Sequence.add('3RD_CORNER', los_move(rectangle_length,rectangle_length)) - Sequence.add('4TH_CORNER', los_move(0,rectangle_length)) - Sequence.add('ORIGIN_RETURN',dp_move(0,0)) + Sequence.add("1ST_CORNER", dp_move(0, 0)) + Sequence.add("2ND_CORNER", los_move(rectangle_length, 0)) + Sequence.add("3RD_CORNER", los_move(rectangle_length, rectangle_length)) + Sequence.add("4TH_CORNER", los_move(0, rectangle_length)) + Sequence.add("ORIGIN_RETURN", dp_move(0, 0)) try: four_corner_sm.execute() - except Exception as e: rospy.loginfo("Pooltest failed: %s" % e) - -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/mission/finite_state_machine/scripts/fsm_helper.py b/mission/finite_state_machine/scripts/fsm_helper.py index 32bf65fae..46f127097 100755 --- a/mission/finite_state_machine/scripts/fsm_helper.py +++ b/mission/finite_state_machine/scripts/fsm_helper.py @@ -22,11 +22,11 @@ from scipy.spatial.transform import Rotation as R import numpy as np -guidance_interface_dp_action_server=rospy.get_param("/guidance/dp/action_server") -guidance_interface_los_action_server=rospy.get_param("/guidance/LOS/action_server") +guidance_interface_dp_action_server = rospy.get_param("/guidance/dp/action_server") +guidance_interface_los_action_server = rospy.get_param("/guidance/LOS/action_server") -#DP control modes +# DP control modes class ControlModeEnum(IntEnum): OPEN_LOOP = 0 POSITION_HOLD = 1 @@ -39,35 +39,46 @@ class ControlModeEnum(IntEnum): ORIENTATION_HOLD = 8 ORIENTATION_DEPTH_HOLD = 9 + def dp_move(x, y, z=-0.5, yaw_rad=0): goal = MoveBaseGoal() - goal.target_pose.pose.position = Point(x,y,z) - goal.target_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, yaw_rad)) + goal.target_pose.pose.position = Point(x, y, z) + goal.target_pose.pose.orientation = Quaternion( + *quaternion_from_euler(0, 0, yaw_rad) + ) + + return SimpleActionState( + guidance_interface_dp_action_server, MoveBaseAction, goal=goal + ) - return SimpleActionState(guidance_interface_dp_action_server, MoveBaseAction, goal=goal) def los_move(x, y, z=-0.5): goal = LosPathFollowingGoal() - goal.next_waypoint = Point(x,y,z) - goal.forward_speed = rospy.get_param('~transit_speed', 0.3) - goal.sphereOfAcceptance = rospy.get_param('~sphere_of_acceptance', 0.5) + goal.next_waypoint = Point(x, y, z) + goal.forward_speed = rospy.get_param("~transit_speed", 0.3) + goal.sphereOfAcceptance = rospy.get_param("~sphere_of_acceptance", 0.5) goal.desired_depth = z - return SimpleActionState(guidance_interface_los_action_server, LosPathFollowingAction, goal=goal) + return SimpleActionState( + guidance_interface_los_action_server, LosPathFollowingAction, goal=goal + ) def circle_move(target_point, direction): goal = MoveGoal() - goal.guidance_type = '' + goal.guidance_type = "" + -def rotate_certain_angle(pose, angle): - '''Angle in degrees''' - - orientation = R.from_quat([pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w]) - rotation = R.from_rotvec(angle*math.pi/180*np.array([0,0,1])) - new_orientation = R.as_quat(orientation*rotation) +def rotate_certain_angle(pose, angle): + """Angle in degrees""" + + orientation = R.from_quat( + [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] + ) + rotation = R.from_rotvec(angle * math.pi / 180 * np.array([0, 0, 1])) + new_orientation = R.as_quat(orientation * rotation) new_pose = Pose() new_pose.position = pose.position new_pose.orientation.x = new_orientation[0] @@ -80,12 +91,14 @@ def rotate_certain_angle(pose, angle): def get_pose_in_front(pose, distance): # returns pose that is distance meters in front of object pose - - orientation_object = R.from_quat([pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w]) + + orientation_object = R.from_quat( + [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] + ) rotation_matrix = orientation_object.as_dcm() - x_vec = rotation_matrix[:,0] + x_vec = rotation_matrix[:, 0] current_pos_vec = np.array([pose.position.x, pose.position.y, pose.position.z]) - new_pose_vec = current_pos_vec + distance*x_vec + new_pose_vec = current_pos_vec + distance * x_vec new_pose = Pose() new_pose.position.x = new_pose_vec[0] @@ -95,22 +108,24 @@ def get_pose_in_front(pose, distance): return new_pose + def get_pose_to_side(pose, unsigned_distance, chosen_side): # returns pose that is distance meters to one side of the gate # chosen_side is either Bootlegger or G-man # Bootlegger (False) = right # G-man (True) = left - orientation_object = R.from_quat([pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w]) + orientation_object = R.from_quat( + [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] + ) rotation_matrix = orientation_object.as_dcm() - y_vec = rotation_matrix[:,1] + y_vec = rotation_matrix[:, 1] current_pos_vec = np.array([pose.position.x, pose.position.y, pose.position.z]) if chosen_side == True: - new_pose_vec = current_pos_vec + unsigned_distance*y_vec + new_pose_vec = current_pos_vec + unsigned_distance * y_vec else: - new_pose_vec = current_pos_vec - unsigned_distance*y_vec - + new_pose_vec = current_pos_vec - unsigned_distance * y_vec new_pose = Pose() new_pose.position.x = new_pose_vec[0] @@ -120,24 +135,27 @@ def get_pose_to_side(pose, unsigned_distance, chosen_side): return new_pose + def patrol_sequence(action_states): - sm = Sequence(outcomes=['preempted', 'succeeded', 'aborted'], connector_outcome='succeeded') + sm = Sequence( + outcomes=["preempted", "succeeded", "aborted"], connector_outcome="succeeded" + ) counter = 0 with sm: for state in action_states: counter = counter + 1 - sm.add("State-%d" %counter, state) + sm.add("State-%d" % counter, state) return sm @cb_interface( - outcomes=['alligned', 'wrong_direction'], - input_keys=['alligned_pose'], - output_keys=['alligned_pose'] + outcomes=["alligned", "wrong_direction"], + input_keys=["alligned_pose"], + output_keys=["alligned_pose"], ) def allignment_checker(userdata): # TODO @@ -146,7 +164,7 @@ def allignment_checker(userdata): def allign_with_target(target): """ - Returns a state that alligns with the specified target. + Returns a state that alligns with the specified target. target: string on the form 'GATE' or 'BOUY' @@ -160,44 +178,40 @@ def allign_with_target(target): move_goal = None allignment_attempt = Concurrence( - outcomes=['succeeded', 'preempted', 'wrong_direction'], + outcomes=["succeeded", "preempted", "wrong_direction"], outcome_map={ - 'succeeded': {'ALLIGNMENT_CHECKER': 'alligned'}, - 'wrong_direction': {'ALLIGNMENT_CHECKER': 'wrong_direction'} + "succeeded": {"ALLIGNMENT_CHECKER": "alligned"}, + "wrong_direction": {"ALLIGNMENT_CHECKER": "wrong_direction"}, }, - default_outcome=['preempted'], - child_termination_cb=None # TODO: should allways terminate + default_outcome=["preempted"], + child_termination_cb=None, # TODO: should allways terminate ) with allignment_attempt: Concurrence.add( - 'CIRCLE_GATE', - SimpleActionState('controller/move', MoveAction, goal=move_goal) # TODO - ) - Concurrence.add( - 'ALLIGNMENT_CHECKER', - CBState(allignment_checker) + "CIRCLE_GATE", + SimpleActionState("controller/move", MoveAction, goal=move_goal), # TODO ) + Concurrence.add("ALLIGNMENT_CHECKER", CBState(allignment_checker)) - -def create_circle_coordinates(start, centre, angle, counterclockwise = True): - resolution = 0.1 #distance between points +def create_circle_coordinates(start, centre, angle, counterclockwise=True): + resolution = 0.1 # distance between points coordinates = [] - radius = math.sqrt(abs((start.x - centre.x)**2) + abs((centre.y - start.y)**2)) - circumference = 2*radius*math.pi - number_of_points = int(math.ceil(circumference/resolution)) - angle_to_add = angle/number_of_points - x = start.x-centre.x - y = start.y-centre.y - start_angle = math.atan2(y,x)*180/math.pi + radius = math.sqrt(abs((start.x - centre.x) ** 2) + abs((centre.y - start.y) ** 2)) + circumference = 2 * radius * math.pi + number_of_points = int(math.ceil(circumference / resolution)) + angle_to_add = angle / number_of_points + x = start.x - centre.x + y = start.y - centre.y + start_angle = math.atan2(y, x) * 180 / math.pi if not counterclockwise: start_angle -= angle - for i in range(number_of_points + 1): - coordX = centre.x + radius*math.cos(start_angle*math.pi/180) - coordY = centre.y + radius*math.sin(start_angle*math.pi/180) - coordinates.append(Point(coordX, coordY,centre.z)) + for i in range(number_of_points + 1): + coordX = centre.x + radius * math.cos(start_angle * math.pi / 180) + coordY = centre.y + radius * math.sin(start_angle * math.pi / 180) + coordinates.append(Point(coordX, coordY, centre.z)) start_angle += angle_to_add if not counterclockwise: return coordinates[::-1] @@ -205,62 +219,62 @@ def create_circle_coordinates(start, centre, angle, counterclockwise = True): return coordinates -def within_acceptance_margins(setpoint, odom_msg, check_yaw_only = False): - if check_yaw_only: - acceptance_margins = [100, 100, 100, 100, 100, 0.1] - acceptance_velocities = [100, 100, 100, 100, 100,100] - - else: - acceptance_margins = rospy.get_param( - "/guidance/dp/acceptance_margins", [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] - ) - acceptance_velocities = rospy.get_param( +def within_acceptance_margins(setpoint, odom_msg, check_yaw_only=False): + if check_yaw_only: + acceptance_margins = [100, 100, 100, 100, 100, 0.1] + acceptance_velocities = [100, 100, 100, 100, 100, 100] + + else: + acceptance_margins = rospy.get_param( + "/guidance/dp/acceptance_margins", [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] + ) + acceptance_velocities = rospy.get_param( "/guidance/dp/acceptance_velocities", [0.01, 0.01, 0.01, 0.01, 0.01, 0.001] - ) - - current_pose = odom_msg.pose.pose - - current_velocity_list = [ - odom_msg.twist.twist.linear.x, - odom_msg.twist.twist.linear.y, - odom_msg.twist.twist.linear.z, - odom_msg.twist.twist.angular.x, - odom_msg.twist.twist.angular.y, - odom_msg.twist.twist.angular.z - ] - - # create quats from msg - goal_quat_list = [ - setpoint.orientation.x, - setpoint.orientation.y, - setpoint.orientation.z, - -setpoint.orientation.w, # invert goal quat - ] - current_quat_list = [ - current_pose.orientation.x, - current_pose.orientation.y, - current_pose.orientation.z, - current_pose.orientation.w, - ] - q_r = quaternion_multiply(current_quat_list, goal_quat_list) - - # convert relative quat to euler - (roll_diff, pitch_diff, yaw_diff) = euler_from_quaternion(q_r) - - # check if close to goal - diff_list = [ - abs(setpoint.position.x - current_pose.position.x), - abs(setpoint.position.y - current_pose.position.y), - abs(setpoint.position.z - current_pose.position.z), - abs(roll_diff), - abs(pitch_diff), - abs(yaw_diff), - ] - is_close = True - for i in range(len(diff_list)): - if diff_list[i] > acceptance_margins[i]: - is_close = False - elif current_velocity_list[i] > acceptance_velocities[i]: - is_close = False - - return is_close + ) + + current_pose = odom_msg.pose.pose + + current_velocity_list = [ + odom_msg.twist.twist.linear.x, + odom_msg.twist.twist.linear.y, + odom_msg.twist.twist.linear.z, + odom_msg.twist.twist.angular.x, + odom_msg.twist.twist.angular.y, + odom_msg.twist.twist.angular.z, + ] + + # create quats from msg + goal_quat_list = [ + setpoint.orientation.x, + setpoint.orientation.y, + setpoint.orientation.z, + -setpoint.orientation.w, # invert goal quat + ] + current_quat_list = [ + current_pose.orientation.x, + current_pose.orientation.y, + current_pose.orientation.z, + current_pose.orientation.w, + ] + q_r = quaternion_multiply(current_quat_list, goal_quat_list) + + # convert relative quat to euler + (roll_diff, pitch_diff, yaw_diff) = euler_from_quaternion(q_r) + + # check if close to goal + diff_list = [ + abs(setpoint.position.x - current_pose.position.x), + abs(setpoint.position.y - current_pose.position.y), + abs(setpoint.position.z - current_pose.position.z), + abs(roll_diff), + abs(pitch_diff), + abs(yaw_diff), + ] + is_close = True + for i in range(len(diff_list)): + if diff_list[i] > acceptance_margins[i]: + is_close = False + elif current_velocity_list[i] > acceptance_velocities[i]: + is_close = False + + return is_close diff --git a/mission/finite_state_machine/scripts/gate.py b/mission/finite_state_machine/scripts/gate.py index 736b5d4bd..729eec247 100644 --- a/mission/finite_state_machine/scripts/gate.py +++ b/mission/finite_state_machine/scripts/gate.py @@ -7,59 +7,81 @@ import actionlib from actionlib_msgs.msg import GoalStatus from move_base_msgs.msg import MoveBaseAction, MoveBaseActionGoal, MoveBaseGoal -from vortex_msgs.msg import VtfPathFollowingAction, VtfPathFollowingGoal, SetVelocityGoal, SetVelocityAction, DpSetpoint, ObjectPosition +from vortex_msgs.msg import ( + VtfPathFollowingAction, + VtfPathFollowingGoal, + SetVelocityGoal, + SetVelocityAction, + DpSetpoint, + ObjectPosition, +) from nav_msgs.msg import Odometry from tf.transformations import quaternion_from_euler -from fsm_helper import dp_move, get_pose_in_front, rotate_certain_angle, within_acceptance_margins +from fsm_helper import ( + dp_move, + get_pose_in_front, + rotate_certain_angle, + within_acceptance_margins, +) from vortex_msgs.srv import ControlMode, SetVelocity class GateSearch(smach.State): def __init__(self): - smach.State.__init__(self, outcomes=['preempted', 'succeeded', 'aborted']) + smach.State.__init__(self, outcomes=["preempted", "succeeded", "aborted"]) - self.landmarks_client = rospy.ServiceProxy('send_positions',request_position) - rospy.wait_for_service('send_positions') + self.landmarks_client = rospy.ServiceProxy("send_positions", request_position) + rospy.wait_for_service("send_positions") self.object = self.landmarks_client("gate").object - desired_velocity_topic = rospy.get_param("/controllers/velocity_controller/desired_velocity_topic") - self.velocity_ctrl_client = rospy.ServiceProxy(desired_velocity_topic,SetVelocity) + desired_velocity_topic = rospy.get_param( + "/controllers/velocity_controller/desired_velocity_topic" + ) + self.velocity_ctrl_client = rospy.ServiceProxy( + desired_velocity_topic, SetVelocity + ) rospy.wait_for_service(desired_velocity_topic) self.dp_pub = rospy.Publisher("/controllers/dp_data", DpSetpoint, queue_size=1) - self.landmarks_pub = rospy.Publisher('/fsm/object_positions_in',ObjectPosition,queue_size=1) + self.landmarks_pub = rospy.Publisher( + "/fsm/object_positions_in", ObjectPosition, queue_size=1 + ) self.recov_point = self.landmarks_client("recovery_point").object - self.state_pub = rospy.Publisher('/fsm/state',String,queue_size=1) + self.state_pub = rospy.Publisher("/fsm/state", String, queue_size=1) vtf_action_server = "/controllers/vtf_action_server" - self.vtf_client = actionlib.SimpleActionClient(vtf_action_server, VtfPathFollowingAction) + self.vtf_client = actionlib.SimpleActionClient( + vtf_action_server, VtfPathFollowingAction + ) rospy.Subscriber("/odometry/filtered", Odometry, self.odom_cb) self.odom = Odometry() def odom_cb(self, msg): - self.odom = msg + self.odom = msg - def execute(self, userdata): self.state_pub.publish("gate_search") rate = rospy.Rate(10) - #RECOVERY - if (self.recov_point.isDetected): + # RECOVERY + if self.recov_point.isDetected: goal = VtfPathFollowingGoal() goal.waypoints = [self.recov_point.objectPose.pose.position] goal.forward_speed = rospy.get_param("/fsm/fast_speed") goal.heading = "path_dependent_heading" - + self.vtf_client.wait_for_server() self.vtf_client.send_goal(goal) - while self.vtf_client.simple_state != actionlib.simple_action_client.SimpleGoalState.DONE: + while ( + self.vtf_client.simple_state + != actionlib.simple_action_client.SimpleGoalState.DONE + ): print("Recovering") rate.sleep() @@ -69,18 +91,22 @@ def execute(self, userdata): self.object.estimateFucked = False self.landmarks_pub.publish(self.object) - + while not self.object.isDetected: - #SEARCH PATTERN + # SEARCH PATTERN goal = VtfPathFollowingGoal() - goal.waypoints = [get_pose_in_front(self.odom.pose.pose,1).position] - goal.waypoints[0].z = - 1.1 + goal.waypoints = [get_pose_in_front(self.odom.pose.pose, 1).position] + goal.waypoints[0].z = -1.1 goal.forward_speed = rospy.get_param("/fsm/medium_speed") goal.heading = "path_dependent_heading" self.vtf_client.wait_for_server() self.vtf_client.send_goal(goal) - while self.vtf_client.simple_state != actionlib.simple_action_client.SimpleGoalState.DONE and not self.object.isDetected: + while ( + self.vtf_client.simple_state + != actionlib.simple_action_client.SimpleGoalState.DONE + and not self.object.isDetected + ): self.object = self.landmarks_client("gate").object print("SEARCHING FOR GATE ...") rate.sleep() @@ -90,96 +116,116 @@ def execute(self, userdata): goal = Pose() goal.position = self.odom.pose.pose.position goal.orientation = self.odom.pose.pose.orientation - goal = rotate_certain_angle(goal,45) + goal = rotate_certain_angle(goal, 45) vel_goal = Twist() vel_goal.angular.z = rospy.get_param("/fsm/turn_speed") - vel_goal.linear.z = -0.01 # should be ommited if drone is balanced and level underwater - vel_goal.linear.x = 0.01 # should be ommited if drone is balanced and level underwater. Same other places. - self.velocity_ctrl_client(vel_goal,True) - while not within_acceptance_margins(goal,self.odom, True) and not self.object.isDetected: + vel_goal.linear.z = ( + -0.01 + ) # should be ommited if drone is balanced and level underwater + vel_goal.linear.x = 0.01 # should be ommited if drone is balanced and level underwater. Same other places. + self.velocity_ctrl_client(vel_goal, True) + while ( + not within_acceptance_margins(goal, self.odom, True) + and not self.object.isDetected + ): self.object = self.landmarks_client("gate").object print("SEARCHING FOR GATE ...") rate.sleep() - self.velocity_ctrl_client(vel_goal,False) + self.velocity_ctrl_client(vel_goal, False) if self.object.isDetected: break goal.position = self.odom.pose.pose.position goal.orientation = self.odom.pose.pose.orientation - goal = rotate_certain_angle(goal,-90) + goal = rotate_certain_angle(goal, -90) vel_goal = Twist() vel_goal.angular.z = -rospy.get_param("/fsm/turn_speed") vel_goal.linear.z = -0.01 vel_goal.linear.x = 0.01 - self.velocity_ctrl_client(vel_goal,True) - while not within_acceptance_margins(goal,self.odom, True) and not self.object.isDetected: + self.velocity_ctrl_client(vel_goal, True) + while ( + not within_acceptance_margins(goal, self.odom, True) + and not self.object.isDetected + ): self.object = self.landmarks_client("gate").object print("SEARCHING FOR GATE ...") rate.sleep() - self.velocity_ctrl_client(vel_goal,False) + self.velocity_ctrl_client(vel_goal, False) if self.object.isDetected: break goal.position = self.odom.pose.pose.position goal.orientation = self.odom.pose.pose.orientation - goal = rotate_certain_angle(goal,45) + goal = rotate_certain_angle(goal, 45) vel_goal = Twist() vel_goal.angular.z = rospy.get_param("/fsm/turn_speed") vel_goal.linear.z = -0.01 vel_goal.linear.x = 0.01 - self.velocity_ctrl_client(vel_goal,True) - while not within_acceptance_margins(goal,self.odom, True) and not self.object.isDetected: + self.velocity_ctrl_client(vel_goal, True) + while ( + not within_acceptance_margins(goal, self.odom, True) + and not self.object.isDetected + ): self.object = self.landmarks_client("gate").object print("SEARCHING FOR GATE ...") rate.sleep() - self.velocity_ctrl_client(vel_goal,False) + self.velocity_ctrl_client(vel_goal, False) if self.object.isDetected: break - print("SEARCHING FOR GATE ...") - rospy.wait_for_service('send_positions') + rospy.wait_for_service("send_positions") self.object = self.landmarks_client("gate").object rate.sleep() - + self.vtf_client.cancel_all_goals() - print("GATE POSITION DETECTED: "+ - str(self.object.objectPose.pose.position.x) + ", "+ - str(self.object.objectPose.pose.position.y) + ", "+ - str(self.object.objectPose.pose.position.z)) - return 'succeeded' - + print( + "GATE POSITION DETECTED: " + + str(self.object.objectPose.pose.position.x) + + ", " + + str(self.object.objectPose.pose.position.y) + + ", " + + str(self.object.objectPose.pose.position.z) + ) + return "succeeded" -class GateConverge(smach.State): + +class GateConverge(smach.State): def __init__(self): - smach.State.__init__(self, outcomes=['preempted', 'succeeded', 'aborted'],output_keys=['gate_converge_output']) - - self.landmarks_client = rospy.ServiceProxy('send_positions',request_position) - rospy.wait_for_service('send_positions') + smach.State.__init__( + self, + outcomes=["preempted", "succeeded", "aborted"], + output_keys=["gate_converge_output"], + ) + + self.landmarks_client = rospy.ServiceProxy("send_positions", request_position) + rospy.wait_for_service("send_positions") self.object = self.landmarks_client("gate").object self.dp_pub = rospy.Publisher("/controllers/dp_data", DpSetpoint, queue_size=1) - self.state_pub = rospy.Publisher('/fsm/state',String,queue_size=1) + self.state_pub = rospy.Publisher("/fsm/state", String, queue_size=1) vtf_action_server = "/controllers/vtf_action_server" - self.vtf_client = actionlib.SimpleActionClient(vtf_action_server, VtfPathFollowingAction) + self.vtf_client = actionlib.SimpleActionClient( + vtf_action_server, VtfPathFollowingAction + ) rospy.Subscriber("/odometry/filtered", Odometry, self.odom_cb) self.odom = Odometry() def odom_cb(self, msg): - self.odom = msg + self.odom = msg def execute(self, userdata): self.state_pub.publish("gate_converge") goal = VtfPathFollowingGoal() self.object = self.landmarks_client("gate").object - goal_pose = get_pose_in_front(self.object.objectPose.pose, 0.5) + goal_pose = get_pose_in_front(self.object.objectPose.pose, 0.5) print("get_pose_in_front returned:") print(goal_pose) - goal.waypoints =[goal_pose.position] + goal.waypoints = [goal_pose.position] goal.forward_speed = rospy.get_param("/fsm/fast_speed") goal.heading = "path_dependent_heading" @@ -187,61 +233,81 @@ def execute(self, userdata): self.vtf_client.send_goal(goal) rate = rospy.Rate(1) rate.sleep() - #TODO: The commented out code below should be there. + # TODO: The commented out code below should be there. # However, the VTF action server prematurely finishes when it is. Investigate this. while not rospy.is_shutdown(): - if self.vtf_client.simple_state == actionlib.simple_action_client.SimpleGoalState.DONE: + if ( + self.vtf_client.simple_state + == actionlib.simple_action_client.SimpleGoalState.DONE + ): break self.object = self.landmarks_client("gate").object # goal.waypoints = [self.object.objectPose.pose.position] - print("GATE POSITION DETECTED: "+ str(goal.waypoints[0].x) + ", "+ str(goal.waypoints[0].y)+ ", "+ str(goal.waypoints[0].z)) + print( + "GATE POSITION DETECTED: " + + str(goal.waypoints[0].x) + + ", " + + str(goal.waypoints[0].y) + + ", " + + str(goal.waypoints[0].z) + ) # goal.waypoints[0] = get_pose_in_front(self.object.objectPose.pose, 0.5).position # self.vtf_client.send_goal(goal) - userdata.gate_converge_output=self.object + userdata.gate_converge_output = self.object rate.sleep() if self.object.estimateFucked: self.vtf_client.cancel_all_goals() - return 'aborted' + return "aborted" self.vtf_client.cancel_all_goals() dp_goal = DpSetpoint() - dp_goal.control_mode = 7 # POSE_HOLD + dp_goal.control_mode = 7 # POSE_HOLD dp_goal.setpoint = self.odom.pose.pose self.dp_pub.publish(dp_goal) while not rospy.is_shutdown() and not self.object.estimateConverged: print("in dp hold") self.object = self.landmarks_client("gate").object if self.object.estimateFucked: - dp_goal.control_mode = 0 # OPEN_LOOP + dp_goal.control_mode = 0 # OPEN_LOOP self.dp_pub.publish(dp_goal) - return 'aborted' + return "aborted" rate.sleep() dp_goal = DpSetpoint() - dp_goal.control_mode = 0 # OPEN_LOOP + dp_goal.control_mode = 0 # OPEN_LOOP self.dp_pub.publish(dp_goal) self.object = self.landmarks_client("gate").object userdata.gate_converge_output = self.object - print("GATE POSITION ESTIMATE CONVERGED AT: " + str(self.object.objectPose.pose.position.x) + "; " \ - + str(self.object.objectPose.pose.position.y) + "; " \ - + str(self.object.objectPose.pose.position.z)) + print( + "GATE POSITION ESTIMATE CONVERGED AT: " + + str(self.object.objectPose.pose.position.x) + + "; " + + str(self.object.objectPose.pose.position.y) + + "; " + + str(self.object.objectPose.pose.position.z) + ) + + return "succeeded" - return 'succeeded' class GateExecute(smach.State): def __init__(self): - smach.State.__init__(self, outcomes=['preempted', 'succeeded', 'aborted'],input_keys=['gate']) - + smach.State.__init__( + self, outcomes=["preempted", "succeeded", "aborted"], input_keys=["gate"] + ) + vtf_action_server = "/controllers/vtf_action_server" - self.vtf_client = actionlib.SimpleActionClient(vtf_action_server, VtfPathFollowingAction) + self.vtf_client = actionlib.SimpleActionClient( + vtf_action_server, VtfPathFollowingAction + ) - self.state_pub = rospy.Publisher('/fsm/state',String,queue_size=1) + self.state_pub = rospy.Publisher("/fsm/state", String, queue_size=1) def execute(self, userdata): self.state_pub.publish("gate_execute") goal = VtfPathFollowingGoal() - goal_pose = get_pose_in_front(userdata.gate.objectPose.pose,-0.5) - goal.waypoints =[goal_pose.position] + goal_pose = get_pose_in_front(userdata.gate.objectPose.pose, -0.5) + goal.waypoints = [goal_pose.position] goal.forward_speed = rospy.get_param("/fsm/medium_speed") goal.heading = "path_dependent_heading" @@ -250,8 +316,11 @@ def execute(self, userdata): rate = rospy.Rate(1) rate.sleep() while not rospy.is_shutdown(): - if self.vtf_client.simple_state == actionlib.simple_action_client.SimpleGoalState.DONE: + if ( + self.vtf_client.simple_state + == actionlib.simple_action_client.SimpleGoalState.DONE + ): break rate.sleep() - return 'succeeded' \ No newline at end of file + return "succeeded" diff --git a/mission/finite_state_machine/scripts/gate_choose_side.py b/mission/finite_state_machine/scripts/gate_choose_side.py index 47f0727fb..ab2ed7ec1 100644 --- a/mission/finite_state_machine/scripts/gate_choose_side.py +++ b/mission/finite_state_machine/scripts/gate_choose_side.py @@ -6,32 +6,45 @@ from std_msgs.msg import String from landmarks.srv import request_position import actionlib -from vortex_msgs.msg import VtfPathFollowingAction, VtfPathFollowingGoal, SetVelocityGoal, SetVelocityAction, DpSetpoint +from vortex_msgs.msg import ( + VtfPathFollowingAction, + VtfPathFollowingGoal, + SetVelocityGoal, + SetVelocityAction, + DpSetpoint, +) from nav_msgs.msg import Odometry from tf.transformations import quaternion_from_euler -from fsm_helper import dp_move, get_pose_in_front, rotate_certain_angle, get_pose_to_side +from fsm_helper import ( + dp_move, + get_pose_in_front, + rotate_certain_angle, + get_pose_to_side, +) from vortex_msgs.srv import ControlMode + class GateSearch(smach.State): def __init__(self): - smach.State.__init__(self, outcomes=['preempted', 'succeeded', 'aborted']) - self.landmarks_client = rospy.ServiceProxy('send_positions',request_position) - rospy.wait_for_service('send_positions') + smach.State.__init__(self, outcomes=["preempted", "succeeded", "aborted"]) + self.landmarks_client = rospy.ServiceProxy("send_positions", request_position) + rospy.wait_for_service("send_positions") self.object = self.landmarks_client("gate").object - self.state_pub = rospy.Publisher('/fsm/state',String,queue_size=1) + self.state_pub = rospy.Publisher("/fsm/state", String, queue_size=1) vtf_action_server = "/controllers/vtf_action_server" - self.vtf_client = actionlib.SimpleActionClient(vtf_action_server, VtfPathFollowingAction) + self.vtf_client = actionlib.SimpleActionClient( + vtf_action_server, VtfPathFollowingAction + ) - def execute(self, userdata): self.state_pub.publish("gate_search") goal = VtfPathFollowingGoal() - goal.waypoints = [Point(5,0,-0.5)] + goal.waypoints = [Point(5, 0, -0.5)] goal.forward_speed = rospy.get_param("/fsm/medium_speed") goal.heading = "path_dependent_heading" self.vtf_client.wait_for_server() @@ -42,50 +55,60 @@ def execute(self, userdata): print("SEARCHING FOR GATE ...") print(self.object.objectPose.pose.position) - rospy.wait_for_service('send_positions') + rospy.wait_for_service("send_positions") self.object = self.landmarks_client("gate").object rate.sleep() - + self.vtf_client.cancel_all_goals() - print("GATE POSITION DETECTED: "+ - str(self.object.objectPose.pose.position.x) + ", "+ - str(self.object.objectPose.pose.position.y) + ", "+ - str(self.object.objectPose.pose.position.z)) - return 'succeeded' - - -class GateConverge(smach.State): + print( + "GATE POSITION DETECTED: " + + str(self.object.objectPose.pose.position.x) + + ", " + + str(self.object.objectPose.pose.position.y) + + ", " + + str(self.object.objectPose.pose.position.z) + ) + return "succeeded" + + +class GateConverge(smach.State): def __init__(self): - smach.State.__init__(self, outcomes=['preempted', 'succeeded', 'aborted'],output_keys=['gate_converge_output']) - - self.landmarks_client = rospy.ServiceProxy('send_positions',request_position) - rospy.wait_for_service('send_positions') + smach.State.__init__( + self, + outcomes=["preempted", "succeeded", "aborted"], + output_keys=["gate_converge_output"], + ) + + self.landmarks_client = rospy.ServiceProxy("send_positions", request_position) + rospy.wait_for_service("send_positions") self.object = self.landmarks_client("gate").object self.g_man_side = True vtf_action_server = "/controllers/vtf_action_server" - self.vtf_client = actionlib.SimpleActionClient(vtf_action_server, VtfPathFollowingAction) + self.vtf_client = actionlib.SimpleActionClient( + vtf_action_server, VtfPathFollowingAction + ) self.dp_pub = rospy.Publisher("/controllers/dp_data", DpSetpoint, queue_size=1) - self.state_pub = rospy.Publisher('/fsm/state',String,queue_size=1) + self.state_pub = rospy.Publisher("/fsm/state", String, queue_size=1) rospy.Subscriber("/odometry/filtered", Odometry, self.odom_cb) self.odom = Odometry() def odom_cb(self, msg): - self.odom = msg + self.odom = msg def execute(self, userdata): self.state_pub.publish("gate_converge") - + goal = VtfPathFollowingGoal() self.object = self.landmarks_client("gate").object goal_pose = get_pose_in_front(self.object.objectPose.pose, 0.5) print("get_pose_in_front returned:") print(goal_pose) - goal.waypoints =[goal_pose.position] + goal.waypoints = [goal_pose.position] goal.forward_speed = rospy.get_param("/fsm/fast_speed") goal.heading = "path_dependent_heading" @@ -94,57 +117,79 @@ def execute(self, userdata): rate = rospy.Rate(1) rate.sleep() while not rospy.is_shutdown(): - if self.vtf_client.simple_state == actionlib.simple_action_client.SimpleGoalState.DONE: + if ( + self.vtf_client.simple_state + == actionlib.simple_action_client.SimpleGoalState.DONE + ): break self.object = self.landmarks_client("gate").object goal.waypoints = [self.object.objectPose.pose.position] - print("GATE POSITION DETECTED: "+ str(goal.waypoints[0].x) + ", "+ str(goal.waypoints[0].y)+ ", "+ str(goal.waypoints[0].z)) + print( + "GATE POSITION DETECTED: " + + str(goal.waypoints[0].x) + + ", " + + str(goal.waypoints[0].y) + + ", " + + str(goal.waypoints[0].z) + ) goal.waypoints[0] = get_pose_in_front(self.object.objectPose.pose, 0.5) - goal.waypoints[0] = get_pose_to_side(goal.waypoints[0], 0.25, self.g_man_side).position + goal.waypoints[0] = get_pose_to_side( + goal.waypoints[0], 0.25, self.g_man_side + ).position self.vtf_client.send_goal(goal) - userdata.gate_converge_output=self.object + userdata.gate_converge_output = self.object rate.sleep() if self.object.estimateFucked: self.vtf_client.cancel_all_goals() - return 'aborted' + return "aborted" self.vtf_client.cancel_all_goals() dp_goal = DpSetpoint() - dp_goal.control_mode = 7 # POSE_HOLD - dp_goal.setpoint = self.odom.pose.pose + dp_goal.control_mode = 7 # POSE_HOLD + dp_goal.setpoint = self.odom.pose.pose self.dp_pub.publish(dp_goal) while not rospy.is_shutdown() and not self.object.estimateConverged: self.object = self.landmarks_client("gate").object if self.object.estimateFucked: - dp_goal.control_mode = 0 # OPEN_LOOP + dp_goal.control_mode = 0 # OPEN_LOOP self.dp_pub.publish(dp_goal) - return 'aborted' + return "aborted" rate.sleep() - dp_goal.control_mode = 0 # OPEN_LOOP + dp_goal.control_mode = 0 # OPEN_LOOP self.dp_pub.publish(dp_goal) self.object = self.landmarks_client("gate").object - userdata.gate_converge_output=self.object - print("GATE POSITION ESTIMATE CONVERGED AT: " + str(self.object.objectPose.pose.position.x) + "; " \ - + str(self.object.objectPose.pose.position.y) + "; " \ - + str(self.object.objectPose.pose.position.z)) + userdata.gate_converge_output = self.object + print( + "GATE POSITION ESTIMATE CONVERGED AT: " + + str(self.object.objectPose.pose.position.x) + + "; " + + str(self.object.objectPose.pose.position.y) + + "; " + + str(self.object.objectPose.pose.position.z) + ) + + return "succeeded" - return 'succeeded' class GateExecute(smach.State): def __init__(self): - smach.State.__init__(self, outcomes=['preempted', 'succeeded', 'aborted'],input_keys=['gate']) - + smach.State.__init__( + self, outcomes=["preempted", "succeeded", "aborted"], input_keys=["gate"] + ) + vtf_action_server = "/controllers/vtf_action_server" - self.vtf_client = actionlib.SimpleActionClient(vtf_action_server, VtfPathFollowingAction) + self.vtf_client = actionlib.SimpleActionClient( + vtf_action_server, VtfPathFollowingAction + ) - self.state_pub = rospy.Publisher('/fsm/state',String,queue_size=1) + self.state_pub = rospy.Publisher("/fsm/state", String, queue_size=1) def execute(self, userdata): self.state_pub.publish("gate_execute") goal = VtfPathFollowingGoal() - goal_pose = get_pose_in_front(userdata.gate.objectPose.pose,-0.5) - goal.waypoints =[goal_pose.position] + goal_pose = get_pose_in_front(userdata.gate.objectPose.pose, -0.5) + goal.waypoints = [goal_pose.position] goal.forward_speed = rospy.get_param("/fsm/medium_speed") goal.heading = "path_dependent_heading" @@ -153,8 +198,11 @@ def execute(self, userdata): rate = rospy.Rate(1) rate.sleep() while not rospy.is_shutdown(): - if self.vtf_client.simple_state == actionlib.simple_action_client.SimpleGoalState.DONE: + if ( + self.vtf_client.simple_state + == actionlib.simple_action_client.SimpleGoalState.DONE + ): break rate.sleep() - return 'succeeded' \ No newline at end of file + return "succeeded" diff --git a/mission/finite_state_machine/scripts/go_to_and_inspect_pt_sm.py b/mission/finite_state_machine/scripts/go_to_and_inspect_pt_sm.py index f10378591..75086d3f5 100755 --- a/mission/finite_state_machine/scripts/go_to_and_inspect_pt_sm.py +++ b/mission/finite_state_machine/scripts/go_to_and_inspect_pt_sm.py @@ -17,9 +17,7 @@ from vortex_msgs.msg import MoveAction, MoveGoal - -def makeMoveGoal(contr_name, target_x, target_y, target_z, radius_of_acceptance = 0.2): - +def makeMoveGoal(contr_name, target_x, target_y, target_z, radius_of_acceptance=0.2): """ string controller_name @@ -29,7 +27,7 @@ def makeMoveGoal(contr_name, target_x, target_y, target_z, radius_of_acceptance --- """ - + move_goal = MoveGoal() move_goal.controller_name = contr_name @@ -39,45 +37,53 @@ def makeMoveGoal(contr_name, target_x, target_y, target_z, radius_of_acceptance move_goal.radius_of_acceptance = radius_of_acceptance - return move_goal -class TaskManager(): - +class TaskManager: def __init__(self): - rospy.init_node('move_to_and_inspect_point_sm', anonymous=False) + rospy.init_node("move_to_and_inspect_point_sm", anonymous=False) + hsm = StateMachine(outcomes=["finished statemachine"]) - hsm = StateMachine(outcomes=['finished statemachine']) - with hsm: - StateMachine.add( 'GO_TO_POINT', - SimpleActionState( 'pid_global', - MoveAction, - makeMoveGoal("pid_global_plan", -3.0, 0, -0.5, radius_of_acceptance = 2.0)), - transitions = { "succeeded": 'INSPECT_POINT', - "preempted": 'INSPECT_POINT', - "aborted": 'INSPECT_POINT' }) - StateMachine.add( 'INSPECT_POINT', - SimpleActionState( 'inspect_point', - MoveAction, - makeMoveGoal("inspect_point", -3.0, 0.0, -0.5, radius_of_acceptance=2.0)), - transitions = { 'succeeded': 'INSPECT_POINT', - "preempted": 'INSPECT_POINT', - "aborted": 'INSPECT_POINT' }) - - - - - - - intro_server = IntrospectionServer(str(rospy.get_name()), hsm,'/SM_ROOT') + StateMachine.add( + "GO_TO_POINT", + SimpleActionState( + "pid_global", + MoveAction, + makeMoveGoal( + "pid_global_plan", -3.0, 0, -0.5, radius_of_acceptance=2.0 + ), + ), + transitions={ + "succeeded": "INSPECT_POINT", + "preempted": "INSPECT_POINT", + "aborted": "INSPECT_POINT", + }, + ) + StateMachine.add( + "INSPECT_POINT", + SimpleActionState( + "inspect_point", + MoveAction, + makeMoveGoal( + "inspect_point", -3.0, 0.0, -0.5, radius_of_acceptance=2.0 + ), + ), + transitions={ + "succeeded": "INSPECT_POINT", + "preempted": "INSPECT_POINT", + "aborted": "INSPECT_POINT", + }, + ) + + intro_server = IntrospectionServer(str(rospy.get_name()), hsm, "/SM_ROOT") intro_server.start() hsm.execute() - #patrol.execute() + # patrol.execute() print("State machine execute finished") intro_server.stop() @@ -86,11 +92,9 @@ def shutdown(self): rospy.sleep(10) - - -if __name__ == '__main__': +if __name__ == "__main__": try: TaskManager() rospy.spin() except rospy.ROSInterruptException: - rospy.loginfo("Pathplanning state machine has been finished") \ No newline at end of file + rospy.loginfo("Pathplanning state machine has been finished") diff --git a/mission/finite_state_machine/scripts/goal_pose_server.py b/mission/finite_state_machine/scripts/goal_pose_server.py index 8ce48702f..7152b0474 100755 --- a/mission/finite_state_machine/scripts/goal_pose_server.py +++ b/mission/finite_state_machine/scripts/goal_pose_server.py @@ -3,26 +3,28 @@ import rospy from geometry_msgs.msg import Point import time - + + def goal_position_publisher(): - pub = rospy.Publisher('goal_position', Point, queue_size=10) + pub = rospy.Publisher("goal_position", Point, queue_size=10) - rospy.init_node('perception', anonymous=True) + rospy.init_node("perception", anonymous=True) - rate = rospy.Rate(1) # 1hz + rate = rospy.Rate(1) # 1hz - goal_position = Point(7,1.5,-0.5) + goal_position = Point(7, 1.5, -0.5) while not rospy.is_shutdown(): - - rospy.loginfo("publishing goal position") + + rospy.loginfo("publishing goal position") pub.publish(goal_position) rate.sleep() - -if __name__ == '__main__': + + +if __name__ == "__main__": try: @@ -30,4 +32,4 @@ def goal_position_publisher(): except rospy.ROSInterruptException: - pass \ No newline at end of file + pass diff --git a/mission/finite_state_machine/scripts/path.py b/mission/finite_state_machine/scripts/path.py index 68c283377..a25c39f50 100644 --- a/mission/finite_state_machine/scripts/path.py +++ b/mission/finite_state_machine/scripts/path.py @@ -7,7 +7,13 @@ import actionlib from actionlib_msgs.msg import GoalStatus from move_base_msgs.msg import MoveBaseAction, MoveBaseActionGoal, MoveBaseGoal -from vortex_msgs.msg import VtfPathFollowingAction, VtfPathFollowingGoal, DpSetpoint,SetVelocityGoal, SetVelocityAction +from vortex_msgs.msg import ( + VtfPathFollowingAction, + VtfPathFollowingGoal, + DpSetpoint, + SetVelocityGoal, + SetVelocityAction, +) from tf.transformations import quaternion_from_euler from fsm_helper import dp_move, get_pose_in_front, rotate_certain_angle @@ -17,22 +23,24 @@ class PathSearch(smach.State): def __init__(self): - smach.State.__init__(self, outcomes=['preempted', 'succeeded', 'aborted']) - self.landmarks_client = rospy.ServiceProxy('send_positions',request_position) - rospy.wait_for_service('send_positions') + smach.State.__init__(self, outcomes=["preempted", "succeeded", "aborted"]) + self.landmarks_client = rospy.ServiceProxy("send_positions", request_position) + rospy.wait_for_service("send_positions") self.object = self.landmarks_client("path").object - self.state_pub = rospy.Publisher('/fsm/state',String,queue_size=1) + self.state_pub = rospy.Publisher("/fsm/state", String, queue_size=1) vtf_action_server = "/controllers/vtf_action_server" - self.vtf_client = actionlib.SimpleActionClient(vtf_action_server, VtfPathFollowingAction) + self.vtf_client = actionlib.SimpleActionClient( + vtf_action_server, VtfPathFollowingAction + ) - #TODO: insert search pattern + # TODO: insert search pattern def execute(self, userdata): self.state_pub.publish("path_search") goal = VtfPathFollowingGoal() - goal.waypoints = [Point(5,0,-0.5)] + goal.waypoints = [Point(5, 0, -0.5)] goal.forward_speed = rospy.get_param("/fsm/medium_speed") goal.heading = "path_dependent_heading" self.vtf_client.wait_for_server() @@ -43,35 +51,41 @@ def execute(self, userdata): print("SEARCHING FOR PATH ...") print(self.object.objectPose.pose.position) - rospy.wait_for_service('send_positions') + rospy.wait_for_service("send_positions") self.object = self.landmarks_client("path").object rate.sleep() - + self.vtf_client.cancel_all_goals() - print("PATH POSITION DETECTED: "+ - str(self.object.objectPose.pose.position.x) + ", "+ - str(self.object.objectPose.pose.position.y) + ", "+ - str(self.object.objectPose.pose.position.z)) - return 'succeeded' + print( + "PATH POSITION DETECTED: " + + str(self.object.objectPose.pose.position.x) + + ", " + + str(self.object.objectPose.pose.position.y) + + ", " + + str(self.object.objectPose.pose.position.z) + ) + return "succeeded" + -class PathConverge(smach.State): +class PathConverge(smach.State): def __init__(self): - smach.State.__init__(self, outcomes=['preempted', 'succeeded', 'aborted']) - - self.landmarks_client = rospy.ServiceProxy('send_positions',request_position) - rospy.wait_for_service('send_positions') + smach.State.__init__(self, outcomes=["preempted", "succeeded", "aborted"]) + + self.landmarks_client = rospy.ServiceProxy("send_positions", request_position) + rospy.wait_for_service("send_positions") self.object = self.landmarks_client("path").object vtf_action_server = "/controllers/vtf_action_server" - self.vtf_client = actionlib.SimpleActionClient(vtf_action_server, VtfPathFollowingAction) - + self.vtf_client = actionlib.SimpleActionClient( + vtf_action_server, VtfPathFollowingAction + ) rospy.Subscriber("/odometry/filtered", Odometry, self.odom_cb) self.odom = Odometry() def odom_cb(self, msg): - self.odom = msg + self.odom = msg def execute(self, userdata): @@ -84,64 +98,83 @@ def execute(self, userdata): self.vtf_client.send_goal(goal) rate = rospy.Rate(1) rate.sleep() - while not rospy.is_shutdown() \ - and not self.vtf_client.simple_state == actionlib.simple_action_client.SimpleGoalState.DONE: + while ( + not rospy.is_shutdown() + and not self.vtf_client.simple_state + == actionlib.simple_action_client.SimpleGoalState.DONE + ): self.object = self.landmarks_client("path").object goal.waypoints = [self.object.objectPose.pose.position] - print("PATH POSITION DETECTED: "+ \ - str(goal.waypoints[0].x) + ", "+ str(goal.waypoints[0].y)+ ", "+ str(goal.waypoints[0].z)) + print( + "PATH POSITION DETECTED: " + + str(goal.waypoints[0].x) + + ", " + + str(goal.waypoints[0].y) + + ", " + + str(goal.waypoints[0].z) + ) self.vtf_client.send_goal(goal) rate.sleep() if self.object.estimateFucked: self.vtf_client.cancel_all_goals() - return 'aborted' + return "aborted" self.vtf_client.cancel_all_goals() self.object = self.landmarks_client("path").object - print("PATH POSITION ESTIMATE CONVERGED AT: " + str(self.object.objectPose.pose.position.x) + "; " \ - + str(self.object.objectPose.pose.position.y) + "; " \ - + str(self.object.objectPose.pose.position.z)) - return 'succeeded' - - + print( + "PATH POSITION ESTIMATE CONVERGED AT: " + + str(self.object.objectPose.pose.position.x) + + "; " + + str(self.object.objectPose.pose.position.y) + + "; " + + str(self.object.objectPose.pose.position.z) + ) + return "succeeded" + + class PathExecute(smach.State): def __init__(self): - smach.State.__init__(self, outcomes=['preempted', 'succeeded', 'aborted'], output_keys=['dir_next_task']) - - self.landmarks_client = rospy.ServiceProxy('send_positions',request_position) - rospy.wait_for_service('send_positions') + smach.State.__init__( + self, + outcomes=["preempted", "succeeded", "aborted"], + output_keys=["dir_next_task"], + ) + + self.landmarks_client = rospy.ServiceProxy("send_positions", request_position) + rospy.wait_for_service("send_positions") self.object = self.landmarks_client("path").object vtf_action_server = "/controllers/vtf_action_server" - self.vtf_client = actionlib.SimpleActionClient(vtf_action_server, VtfPathFollowingAction) + self.vtf_client = actionlib.SimpleActionClient( + vtf_action_server, VtfPathFollowingAction + ) - self.dp_pub = rospy.Publisher("controllers/dp_data", DpSetpoint) + self.dp_pub = rospy.Publisher("controllers/dp_data", DpSetpoint) rospy.Subscriber("/odometry/filtered", Odometry, self.odom_cb) self.odom = Odometry() def odom_cb(self, msg): - self.odom = msg + self.odom = msg def execute(self, userdata): dp_goal = DpSetpoint() - dp_goal.control_mode = 7 # POSE_HOLD + dp_goal.control_mode = 7 # POSE_HOLD dp_goal.setpoint = self.odom.pose.pose self.dp_pub.publish(dp_goal) rate = rospy.Rate(1) - while not rospy.is_shutdown()\ - and not self.object.estimateConverged: + while not rospy.is_shutdown() and not self.object.estimateConverged: if self.object.estimateFucked: - dp_goal.control_mode = 0 # OPEN_LOOP + dp_goal.control_mode = 0 # OPEN_LOOP self.dp_pub.publish(dp_goal) - return 'aborted' + return "aborted" print("in dp hold") self.object = self.landmarks_client("path").object dp_goal.setpoint = self.object.objectPose.pose self.dp_pub.publish(dp_goal) rate.sleep() - dp_goal.control_mode = 0 # OPEN_LOOP + dp_goal.control_mode = 0 # OPEN_LOOP self.dp_pub.publish(dp_goal) - + userdata.dir_next_task = self.object - return 'succeeded' + return "succeeded" diff --git a/mission/finite_state_machine/scripts/perception_simulator.py b/mission/finite_state_machine/scripts/perception_simulator.py index dd3c66e40..5021843f0 100755 --- a/mission/finite_state_machine/scripts/perception_simulator.py +++ b/mission/finite_state_machine/scripts/perception_simulator.py @@ -6,16 +6,19 @@ from geometry_msgs.msg import Point import time + class Perciever: def __init__(self): self.state = "da" - rospy.Subscriber("/fsm/state",String,self.state_cb) + rospy.Subscriber("/fsm/state", String, self.state_cb) + + self.landmarks_pub = rospy.Publisher( + "/fsm/object_positions_in", ObjectPosition, queue_size=1 + ) - self.landmarks_pub = rospy.Publisher('/fsm/object_positions_in',ObjectPosition,queue_size=1) - self.gate = ObjectPosition() self.gate.objectID = "gate" - self.gate.objectPose.pose.position = Point(10,0,-1.4) + self.gate.objectPose.pose.position = Point(10, 0, -1.4) self.gate.objectPose.pose.orientation.x = 0 self.gate.objectPose.pose.orientation.y = 0 self.gate.objectPose.pose.orientation.z = 1 @@ -25,7 +28,7 @@ def __init__(self): self.pole = ObjectPosition() self.pole.objectID = "pole" - self.pole.objectPose.pose.position = Point(16.7,0,-1.4) + self.pole.objectPose.pose.position = Point(16.7, 0, -1.4) self.pole.objectPose.pose.orientation.x = 0 self.pole.objectPose.pose.orientation.y = 0 self.pole.objectPose.pose.orientation.z = 1 @@ -57,8 +60,7 @@ def execute(self): self.landmarks_pub.publish(self.gate) -if __name__ == '__main__': - rospy.init_node('perception_simulator') +if __name__ == "__main__": + rospy.init_node("perception_simulator") perceptron = Perciever() perceptron.execute() - \ No newline at end of file diff --git a/mission/finite_state_machine/scripts/pole.py b/mission/finite_state_machine/scripts/pole.py index c30265ffd..75b1f919e 100644 --- a/mission/finite_state_machine/scripts/pole.py +++ b/mission/finite_state_machine/scripts/pole.py @@ -8,49 +8,70 @@ import actionlib from actionlib_msgs.msg import GoalStatus from move_base_msgs.msg import MoveBaseAction, MoveBaseActionGoal, MoveBaseGoal -from vortex_msgs.msg import VtfPathFollowingAction, VtfPathFollowingGoal, SetVelocityGoal, SetVelocityAction, DpSetpoint +from vortex_msgs.msg import ( + VtfPathFollowingAction, + VtfPathFollowingGoal, + SetVelocityGoal, + SetVelocityAction, + DpSetpoint, +) from landmarks.srv import request_position -from fsm_helper import get_pose_in_front, create_circle_coordinates, rotate_certain_angle, within_acceptance_margins +from fsm_helper import ( + get_pose_in_front, + create_circle_coordinates, + rotate_certain_angle, + within_acceptance_margins, +) from vortex_msgs.srv import ControlMode, SetVelocity from nav_msgs.msg import Odometry + class PoleSearch(smach.State): def __init__(self): - smach.State.__init__(self, outcomes=['preempted', 'succeeded', 'aborted']) + smach.State.__init__(self, outcomes=["preempted", "succeeded", "aborted"]) - self.landmarks_client = rospy.ServiceProxy('send_positions',request_position) + self.landmarks_client = rospy.ServiceProxy("send_positions", request_position) self.object = self.landmarks_client("gate").object - desired_velocity_topic = rospy.get_param("/controllers/velocity_controller/desired_velocity_topic") - self.velocity_ctrl_client = rospy.ServiceProxy(desired_velocity_topic,SetVelocity) + desired_velocity_topic = rospy.get_param( + "/controllers/velocity_controller/desired_velocity_topic" + ) + self.velocity_ctrl_client = rospy.ServiceProxy( + desired_velocity_topic, SetVelocity + ) rospy.wait_for_service(desired_velocity_topic) self.dp_pub = rospy.Publisher("/controllers/dp_data", DpSetpoint, queue_size=1) - self.state_pub = rospy.Publisher('/fsm/state',String,queue_size=1) + self.state_pub = rospy.Publisher("/fsm/state", String, queue_size=1) vtf_action_server = "/controllers/vtf_action_server" - self.vtf_client = actionlib.SimpleActionClient(vtf_action_server, VtfPathFollowingAction) + self.vtf_client = actionlib.SimpleActionClient( + vtf_action_server, VtfPathFollowingAction + ) rospy.Subscriber("/odometry/filtered", Odometry, self.odom_cb) self.odom = Odometry() def odom_cb(self, msg): - self.odom = msg + self.odom = msg - def execute(self, userdata): self.state_pub.publish("pole_search") rate = rospy.Rate(10) while not self.object.isDetected: - #SEARCH PATTERN + # SEARCH PATTERN goal = VtfPathFollowingGoal() - goal.waypoints = [get_pose_in_front(self.odom.pose.pose,1).position] + goal.waypoints = [get_pose_in_front(self.odom.pose.pose, 1).position] goal.forward_speed = rospy.get_param("/fsm/medium_speed") goal.heading = "path_dependent_heading" self.vtf_client.wait_for_server() self.vtf_client.send_goal(goal) - while self.vtf_client.simple_state != actionlib.simple_action_client.SimpleGoalState.DONE and not self.object.isDetected: + while ( + self.vtf_client.simple_state + != actionlib.simple_action_client.SimpleGoalState.DONE + and not self.object.isDetected + ): self.object = self.landmarks_client("pole").object print("SEARCHING FOR POLE ...") print(self.object.objectPose.pose.position) @@ -61,89 +82,107 @@ def execute(self, userdata): goal = Pose() goal.position = self.odom.pose.pose.position goal.orientation = self.odom.pose.pose.orientation - goal = rotate_certain_angle(goal,45) + goal = rotate_certain_angle(goal, 45) vel_goal = Twist() vel_goal.angular.z = rospy.get_param("/fsm/turn_speed") - self.velocity_ctrl_client(vel_goal,True) - while not within_acceptance_margins(goal,self.odom, True) and not self.object.isDetected: + self.velocity_ctrl_client(vel_goal, True) + while ( + not within_acceptance_margins(goal, self.odom, True) + and not self.object.isDetected + ): self.object = self.landmarks_client("pole").object print("SEARCHING FOR POLE ...") print(self.object.objectPose.pose.position) rate.sleep() - self.velocity_ctrl_client(vel_goal,False) + self.velocity_ctrl_client(vel_goal, False) if self.object.isDetected: break goal.position = self.odom.pose.pose.position goal.orientation = self.odom.pose.pose.orientation - goal = rotate_certain_angle(goal,-90) + goal = rotate_certain_angle(goal, -90) vel_goal = Twist() vel_goal.angular.z = -rospy.get_param("/fsm/turn_speed") - self.velocity_ctrl_client(vel_goal,True) - while not within_acceptance_margins(goal,self.odom, True) and not self.object.isDetected: + self.velocity_ctrl_client(vel_goal, True) + while ( + not within_acceptance_margins(goal, self.odom, True) + and not self.object.isDetected + ): self.object = self.landmarks_client("pole").object print("SEARCHING FOR POLE ...") print(self.object.objectPose.pose.position) rate.sleep() - self.velocity_ctrl_client(vel_goal,False) + self.velocity_ctrl_client(vel_goal, False) if self.object.isDetected: break goal.position = self.odom.pose.pose.position goal.orientation = self.odom.pose.pose.orientation - goal = rotate_certain_angle(goal,45) + goal = rotate_certain_angle(goal, 45) vel_goal = Twist() vel_goal.angular.z = rospy.get_param("/fsm/turn_speed") - self.velocity_ctrl_client(vel_goal,True) - while not within_acceptance_margins(goal,self.odom, True) and not self.object.isDetected: + self.velocity_ctrl_client(vel_goal, True) + while ( + not within_acceptance_margins(goal, self.odom, True) + and not self.object.isDetected + ): self.object = self.landmarks_client("pole").object print("SEARCHING FOR POLE ...") print(self.object.objectPose.pose.position) rate.sleep() - self.velocity_ctrl_client(vel_goal,False) + self.velocity_ctrl_client(vel_goal, False) if self.object.isDetected: break - print("SEARCHING FOR POLE ...") print(self.object.objectPose.pose.position) - rospy.wait_for_service('send_positions') + rospy.wait_for_service("send_positions") self.object = self.landmarks_client("pole").object rate.sleep() - + self.vtf_client.cancel_all_goals() - print("POLE POSITION DETECTED: "+ - str(self.object.objectPose.pose.position.x) + ", "+ - str(self.object.objectPose.pose.position.y) + ", "+ - str(self.object.objectPose.pose.position.z)) - return 'succeeded' - - + print( + "POLE POSITION DETECTED: " + + str(self.object.objectPose.pose.position.x) + + ", " + + str(self.object.objectPose.pose.position.y) + + ", " + + str(self.object.objectPose.pose.position.z) + ) + return "succeeded" + class PoleConverge(smach.State): def __init__(self): - smach.State.__init__(self, outcomes=['preempted', 'succeeded', 'aborted'], output_keys=['pole_converge_output']) - - self.landmarks_client = rospy.ServiceProxy('send_positions',request_position) - rospy.wait_for_service('send_positions') + smach.State.__init__( + self, + outcomes=["preempted", "succeeded", "aborted"], + output_keys=["pole_converge_output"], + ) + + self.landmarks_client = rospy.ServiceProxy("send_positions", request_position) + rospy.wait_for_service("send_positions") self.object = self.landmarks_client("pole").object - dp_guidance_action_server="/guidance_interface/dp_server" - self.action_client = actionlib.SimpleActionClient(dp_guidance_action_server, MoveBaseAction) + dp_guidance_action_server = "/guidance_interface/dp_server" + self.action_client = actionlib.SimpleActionClient( + dp_guidance_action_server, MoveBaseAction + ) self.dp_pub = rospy.Publisher("/controllers/dp_data", DpSetpoint, queue_size=1) - self.state_pub = rospy.Publisher('/fsm/state',String,queue_size=1) + self.state_pub = rospy.Publisher("/fsm/state", String, queue_size=1) vtf_action_server = "/controllers/vtf_action_server" - self.vtf_client = actionlib.SimpleActionClient(vtf_action_server, VtfPathFollowingAction) + self.vtf_client = actionlib.SimpleActionClient( + vtf_action_server, VtfPathFollowingAction + ) rospy.Subscriber("/odometry/filtered", Odometry, self.odom_cb) self.odom = Odometry() def odom_cb(self, msg): - self.odom = msg - + self.odom = msg def execute(self, userdata): self.state_pub.publish("pole_converge") @@ -152,7 +191,7 @@ def execute(self, userdata): self.object = self.landmarks_client("pole").object goal_pose = get_pose_in_front(self.object.objectPose.pose, 1) - print('get_pose_in_front returned:') + print("get_pose_in_front returned:") print(goal_pose) goal.waypoints = [goal_pose.position] @@ -165,12 +204,22 @@ def execute(self, userdata): rate = rospy.Rate(1) rate.sleep() while not rospy.is_shutdown(): - if self.vtf_client.simple_state == actionlib.simple_action_client.SimpleGoalState.DONE: + if ( + self.vtf_client.simple_state + == actionlib.simple_action_client.SimpleGoalState.DONE + ): break self.object = self.landmarks_client("pole").object - + # goal.waypoints = [self.object.objectPose.pose.position] - print("POLE POSITION DETECTED: "+ str(goal.waypoints[0].x) + ", "+ str(goal.waypoints[0].y)+ ", "+ str(goal.waypoints[0].z)) + print( + "POLE POSITION DETECTED: " + + str(goal.waypoints[0].x) + + ", " + + str(goal.waypoints[0].y) + + ", " + + str(goal.waypoints[0].z) + ) # goal.waypoints[0] = get_pose_in_front(self.object.objectPose.pose, 0.5).position @@ -180,44 +229,54 @@ def execute(self, userdata): if self.object.estimateFucked: self.vtf_client.cancel_all_goals() - return 'aborted' + return "aborted" self.vtf_client.cancel_all_goals() dp_goal = DpSetpoint() - dp_goal.control_mode = 7 # POSE_HOLD + dp_goal.control_mode = 7 # POSE_HOLD dp_goal.setpoint = self.odom.pose.pose self.dp_pub.publish(dp_goal) while not rospy.is_shutdown() and not self.object.estimateConverged: self.object = self.landmarks_client("pole").object if self.object.estimateFucked: - dp_goal.control_mode = 0 # OPEN_LOOP + dp_goal.control_mode = 0 # OPEN_LOOP self.dp_pub.publish(dp_goal) - return 'aborted' + return "aborted" rate.sleep() - dp_goal.control_mode = 0 # OPEN_LOOP + dp_goal.control_mode = 0 # OPEN_LOOP self.dp_pub.publish(dp_goal) self.object = self.landmarks_client("pole").object - userdata.pole_converge_output=self.object - print("POLE POSITION ESTIMATE CONVERGED AT: " + str(self.object.objectPose.pose.position.x) + "; " \ - + str(self.object.objectPose.pose.position.y) + "; " \ - + str(self.object.objectPose.pose.position.z)) + userdata.pole_converge_output = self.object + print( + "POLE POSITION ESTIMATE CONVERGED AT: " + + str(self.object.objectPose.pose.position.x) + + "; " + + str(self.object.objectPose.pose.position.y) + + "; " + + str(self.object.objectPose.pose.position.z) + ) + + return "succeeded" - return 'succeeded' class PoleExecute(smach.State): def __init__(self): - smach.State.__init__(self, outcomes=['preempted', 'succeeded', 'aborted'],input_keys=['pole']) - + smach.State.__init__( + self, outcomes=["preempted", "succeeded", "aborted"], input_keys=["pole"] + ) + vtf_action_server = "/controllers/vtf_action_server" - self.vtf_client = actionlib.SimpleActionClient(vtf_action_server, VtfPathFollowingAction) + self.vtf_client = actionlib.SimpleActionClient( + vtf_action_server, VtfPathFollowingAction + ) - self.state_pub = rospy.Publisher('/fsm/state',String,queue_size=1) + self.state_pub = rospy.Publisher("/fsm/state", String, queue_size=1) rospy.Subscriber("/odometry/filtered", Odometry, self.odom_cb) self.odom = Odometry() def odom_cb(self, msg): - self.odom = msg + self.odom = msg def execute(self, userdata): self.state_pub.publish("pole_execute") @@ -225,13 +284,17 @@ def execute(self, userdata): goal = VtfPathFollowingGoal() start = self.odom.pose.pose.position print(userdata) - centre = Point(userdata.pole.objectPose.pose.position.x,userdata.pole.objectPose.pose.position.y, userdata.pole.objectPose.pose.position.z) - goal.waypoints = create_circle_coordinates(start,centre,330) + centre = Point( + userdata.pole.objectPose.pose.position.x, + userdata.pole.objectPose.pose.position.y, + userdata.pole.objectPose.pose.position.z, + ) + goal.waypoints = create_circle_coordinates(start, centre, 330) goal.forward_speed = rospy.get_param("/fsm/medium_speed") goal.heading_point.x = userdata.pole.objectPose.pose.position.x goal.heading_point.y = userdata.pole.objectPose.pose.position.y goal.heading_point.z = userdata.pole.objectPose.pose.position.z - + goal.heading = "point_dependent_heading" self.vtf_client.wait_for_server() @@ -239,7 +302,10 @@ def execute(self, userdata): rate = rospy.Rate(1) rate.sleep() while not rospy.is_shutdown(): - if self.vtf_client.simple_state == actionlib.simple_action_client.SimpleGoalState.DONE: + if ( + self.vtf_client.simple_state + == actionlib.simple_action_client.SimpleGoalState.DONE + ): break rate.sleep() - return 'succeeded' \ No newline at end of file + return "succeeded" diff --git a/mission/finite_state_machine/scripts/pooltest.py b/mission/finite_state_machine/scripts/pooltest.py index 89f001b67..94974acfc 100755 --- a/mission/finite_state_machine/scripts/pooltest.py +++ b/mission/finite_state_machine/scripts/pooltest.py @@ -29,31 +29,35 @@ def visit_waypoints(): GoToState(pose2), ] ) - introspection_server = IntrospectionServer(str(rospy.get_name()), sm, '/sm_root') + introspection_server = IntrospectionServer(str(rospy.get_name()), sm, "/sm_root") introspection_server.start() sm.execute() + def test_restoring(): twist = Twist() state = VelState(twist) res = state.execute(None) rospy.loginfo(str(res)) + def test_vel(): twist = Twist() twist.linear.x = 0.25 twist.angular.x = 0.0 states = [ GoToState(pose(-1.5, 0, 0.7, 0, 0, 0)), - VelState(twist, dp_control_mode=ControlModeEnum.ORIENTATION_DEPTH_HOLD) + VelState(twist, dp_control_mode=ControlModeEnum.ORIENTATION_DEPTH_HOLD), ] return states + def test_dp(): test_pose = pose(0, 0, 0.7, 0, 0, 0) return DpState(test_pose) + def test_los(): goal_pos = Point() goal_pos.z = 1 diff --git a/mission/finite_state_machine/scripts/prequalification_fsm.py b/mission/finite_state_machine/scripts/prequalification_fsm.py index 8eb30bcc0..b62bc8c41 100755 --- a/mission/finite_state_machine/scripts/prequalification_fsm.py +++ b/mission/finite_state_machine/scripts/prequalification_fsm.py @@ -11,85 +11,84 @@ def main(): - rospy.init_node('prequalification_fsm') + rospy.init_node("prequalification_fsm") rospy.sleep(rospy.get_param("/fsm/time_to_launch")) - rospy.wait_for_service('send_positions') #consider moving into individual state functions - - prequalification_state_machine = StateMachine(outcomes=['preempted', 'succeeded', 'aborted']) - + rospy.wait_for_service( + "send_positions" + ) # consider moving into individual state functions + + prequalification_state_machine = StateMachine( + outcomes=["preempted", "succeeded", "aborted"] + ) + with prequalification_state_machine: - - StateMachine.add('PREQUAL_PREPARE', - ReachDepth(), - transitions={'succeeded':'GATE_SM'}) - - gate_sm = StateMachine(outcomes=['preempted', 'succeeded', 'aborted']) + StateMachine.add( + "PREQUAL_PREPARE", ReachDepth(), transitions={"succeeded": "GATE_SM"} + ) + + gate_sm = StateMachine(outcomes=["preempted", "succeeded", "aborted"]) with gate_sm: - StateMachine.add('GATE_SEARCH', - GateSearch(), - transitions={'succeeded':'GATE_CONVERGE'}) - - - StateMachine.add('GATE_CONVERGE', - GateConverge(), - transitions={'succeeded' : 'GATE_EXECUTE','aborted' : 'GATE_SEARCH'}, - remapping={'gate_converge_output':'gate'}) - - StateMachine.add('GATE_EXECUTE', - GateExecute()) - - - StateMachine.add('GATE_SM',gate_sm, - transitions={'succeeded':'POLE_SM'} ) - - - pole_sm = StateMachine(outcomes=['preempted', 'succeeded', 'aborted']) + StateMachine.add( + "GATE_SEARCH", GateSearch(), transitions={"succeeded": "GATE_CONVERGE"} + ) + + StateMachine.add( + "GATE_CONVERGE", + GateConverge(), + transitions={"succeeded": "GATE_EXECUTE", "aborted": "GATE_SEARCH"}, + remapping={"gate_converge_output": "gate"}, + ) + + StateMachine.add("GATE_EXECUTE", GateExecute()) + + StateMachine.add("GATE_SM", gate_sm, transitions={"succeeded": "POLE_SM"}) + + pole_sm = StateMachine(outcomes=["preempted", "succeeded", "aborted"]) with pole_sm: - StateMachine.add('POLE_SEARCH', - PoleSearch(), - transitions={'succeeded':'POLE_CONVERGE'}) - - StateMachine.add('POLE_CONVERGE', - PoleConverge(), - transitions={'succeeded':'POLE_EXECUTE', 'aborted':'POLE_SEARCH'}, - remapping={'pole_converge_output':'pole'}) + StateMachine.add( + "POLE_SEARCH", PoleSearch(), transitions={"succeeded": "POLE_CONVERGE"} + ) + + StateMachine.add( + "POLE_CONVERGE", + PoleConverge(), + transitions={"succeeded": "POLE_EXECUTE", "aborted": "POLE_SEARCH"}, + remapping={"pole_converge_output": "pole"}, + ) - StateMachine.add('POLE_EXECUTE', - PoleExecute()) + StateMachine.add("POLE_EXECUTE", PoleExecute()) - StateMachine.add('POLE_SM', pole_sm, - transitions={'succeeded':'GATE_SM_BACK'}) + StateMachine.add("POLE_SM", pole_sm, transitions={"succeeded": "GATE_SM_BACK"}) - gate_sm_back = StateMachine(outcomes=['preempted', 'succeeded', 'aborted']) + gate_sm_back = StateMachine(outcomes=["preempted", "succeeded", "aborted"]) with gate_sm_back: - StateMachine.add('GATE_SEARCH', - GateSearch(), - transitions={'succeeded':'GATE_CONVERGE'}) - - - StateMachine.add('GATE_CONVERGE', - GateConverge(), - transitions={'succeeded' : 'GATE_EXECUTE','aborted' : 'GATE_SEARCH'}, - remapping={'gate_converge_output':'gate'}) - - StateMachine.add('GATE_EXECUTE', - GateExecute()) - - - StateMachine.add('GATE_SM_BACK',gate_sm_back) - - - - intro_server = IntrospectionServer(str(rospy.get_name()), prequalification_state_machine,'/SM_ROOT') + StateMachine.add( + "GATE_SEARCH", GateSearch(), transitions={"succeeded": "GATE_CONVERGE"} + ) + + StateMachine.add( + "GATE_CONVERGE", + GateConverge(), + transitions={"succeeded": "GATE_EXECUTE", "aborted": "GATE_SEARCH"}, + remapping={"gate_converge_output": "gate"}, + ) + + StateMachine.add("GATE_EXECUTE", GateExecute()) + + StateMachine.add("GATE_SM_BACK", gate_sm_back) + + intro_server = IntrospectionServer( + str(rospy.get_name()), prequalification_state_machine, "/SM_ROOT" + ) intro_server.start() try: @@ -100,5 +99,5 @@ def main(): rospy.loginfo("Prequalification test failed: %s" % e) -if __name__ == '__main__': - main() \ No newline at end of file +if __name__ == "__main__": + main() diff --git a/mission/finite_state_machine/scripts/reach_depth.py b/mission/finite_state_machine/scripts/reach_depth.py index 9d8afb4a4..c1829ff4b 100644 --- a/mission/finite_state_machine/scripts/reach_depth.py +++ b/mission/finite_state_machine/scripts/reach_depth.py @@ -10,30 +10,36 @@ from vortex_msgs.msg import VtfPathFollowingAction, VtfPathFollowingGoal from landmarks.srv import request_position from tf.transformations import quaternion_from_euler -from vortex_msgs.srv import ControlMode #, ControlModeRequest +from vortex_msgs.srv import ControlMode # , ControlModeRequest from fsm_helper import within_acceptance_margins from nav_msgs.msg import Odometry from fsm_helper import dp_move, los_move -from vortex_msgs.srv import ControlMode#, ControlModeRequest +from vortex_msgs.srv import ControlMode # , ControlModeRequest + class ReachDepth(smach.State): def __init__(self): - smach.State.__init__(self, outcomes=['preempted', 'succeeded', 'aborted']) + smach.State.__init__(self, outcomes=["preempted", "succeeded", "aborted"]) vtf_action_server = "/controllers/vtf_action_server" - self.vtf_client = actionlib.SimpleActionClient(vtf_action_server, VtfPathFollowingAction) + self.vtf_client = actionlib.SimpleActionClient( + vtf_action_server, VtfPathFollowingAction + ) def execute(self, userdata): goal = VtfPathFollowingGoal() - goal.waypoints = [Point(0.1,0,-1)] + goal.waypoints = [Point(0.1, 0, -1)] goal.forward_speed = rospy.get_param("/fsm/medium_speed") goal.heading = "path_dependent_heading" self.vtf_client.wait_for_server() self.vtf_client.send_goal(goal) rate = rospy.Rate(10) while not rospy.is_shutdown(): - if self.vtf_client.simple_state == actionlib.simple_action_client.SimpleGoalState.DONE: + if ( + self.vtf_client.simple_state + == actionlib.simple_action_client.SimpleGoalState.DONE + ): print("ReachDepth succeeded") break rate.sleep() - - return 'succeeded' \ No newline at end of file + + return "succeeded" diff --git a/mission/finite_state_machine/scripts/robosub_fsm.py b/mission/finite_state_machine/scripts/robosub_fsm.py index abdbb11ec..d0f2a25ac 100755 --- a/mission/finite_state_machine/scripts/robosub_fsm.py +++ b/mission/finite_state_machine/scripts/robosub_fsm.py @@ -10,74 +10,83 @@ from buoy import BuoySearch, BuoyConverge, BuoyExecute from reach_depth import ReachDepth + def main(): - rospy.init_node('robosub_fsm') + rospy.init_node("robosub_fsm") + + rospy.wait_for_service( + "send_positions" + ) # consider moving into individual state functions + + robosub_state_machine = StateMachine(outcomes=["preempted", "succeeded", "aborted"]) - rospy.wait_for_service('send_positions') #consider moving into individual state functions - - robosub_state_machine = StateMachine(outcomes=['preempted', 'succeeded', 'aborted']) - with robosub_state_machine: ##COIN FLIP - StateMachine.add('ROBOSUB_PREPARE', - ReachDepth(), - transitions={'succeeded':'GATE_SM'}) + StateMachine.add( + "ROBOSUB_PREPARE", ReachDepth(), transitions={"succeeded": "GATE_SM"} + ) ##GATE - gate_sm = StateMachine(outcomes=['preempted', 'succeeded', 'aborted']) + gate_sm = StateMachine(outcomes=["preempted", "succeeded", "aborted"]) with gate_sm: - StateMachine.add('GATE_SEARCH', - GateSearch(), - transitions={'succeeded':'GATE_CONVERGE'}, - remapping={'gate_search_output':'gate'}) - - StateMachine.add('GATE_CONVERGE', - GateConverge(), - transitions={'succeeded' : 'GATE_EXECUTE','aborted' : 'GATE_SEARCH'}, - remapping={'gate_converge_output':'gate'}) - - StateMachine.add('GATE_EXECUTE', - GateExecute()) - - StateMachine.add('GATE_SM',gate_sm, transitions={'succeeded':'PATH_SM'} ) + StateMachine.add( + "GATE_SEARCH", + GateSearch(), + transitions={"succeeded": "GATE_CONVERGE"}, + remapping={"gate_search_output": "gate"}, + ) + + StateMachine.add( + "GATE_CONVERGE", + GateConverge(), + transitions={"succeeded": "GATE_EXECUTE", "aborted": "GATE_SEARCH"}, + remapping={"gate_converge_output": "gate"}, + ) + + StateMachine.add("GATE_EXECUTE", GateExecute()) + + StateMachine.add("GATE_SM", gate_sm, transitions={"succeeded": "PATH_SM"}) ##PATH - path_sm = StateMachine(outcomes=['preempted', 'succeeded', 'aborted']) + path_sm = StateMachine(outcomes=["preempted", "succeeded", "aborted"]) with path_sm: - StateMachine.add('PATH_SEARCH', - PathSearch(), - transitions={'succeeded':'PATH_CONVERGE'}) - - StateMachine.add('PATH_CONVERGE', - PathConverge(), - transitions={'succeeded' : 'PATH_EXECUTE','aborted' : 'PATH_SEARCH'}) - - StateMachine.add('PATH_EXECUTE', - PathExecute(), - remapping={'dir_next_task' : 'dir_next_task'}, - transitions={'aborted' : 'PATH_SEARCH'} - ) - - StateMachine.add('PATH_SM',path_sm, transitions={'succeeded':'BUOY_SM'}) + StateMachine.add( + "PATH_SEARCH", PathSearch(), transitions={"succeeded": "PATH_CONVERGE"} + ) + + StateMachine.add( + "PATH_CONVERGE", + PathConverge(), + transitions={"succeeded": "PATH_EXECUTE", "aborted": "PATH_SEARCH"}, + ) + + StateMachine.add( + "PATH_EXECUTE", + PathExecute(), + remapping={"dir_next_task": "dir_next_task"}, + transitions={"aborted": "PATH_SEARCH"}, + ) + + StateMachine.add("PATH_SM", path_sm, transitions={"succeeded": "BUOY_SM"}) ##BUOY - buoy_sm = StateMachine(outcomes=['preempted', 'succeeded', 'aborted']) + buoy_sm = StateMachine(outcomes=["preempted", "succeeded", "aborted"]) with buoy_sm: - StateMachine.add('BUOY_SEARCH', - BuoySearch(), - transitions={'succeeded':'BUOY_CONVERGE'}) - - - StateMachine.add('BUOY_CONVERGE', - BuoyConverge(), - transitions={'succeeded' : 'BUOY_EXECUTE','aborted' : 'BUOY_SEARCH'}, - remapping={'buoy_converge_output':'buoy'}) - - StateMachine.add('BUOY_EXECUTE', - BuoyExecute()) - - StateMachine.add('BUOY_SM',buoy_sm) + StateMachine.add( + "BUOY_SEARCH", BuoySearch(), transitions={"succeeded": "BUOY_CONVERGE"} + ) + + StateMachine.add( + "BUOY_CONVERGE", + BuoyConverge(), + transitions={"succeeded": "BUOY_EXECUTE", "aborted": "BUOY_SEARCH"}, + remapping={"buoy_converge_output": "buoy"}, + ) + + StateMachine.add("BUOY_EXECUTE", BuoyExecute()) + + StateMachine.add("BUOY_SM", buoy_sm) ##TORPEDO @@ -85,8 +94,9 @@ def main(): ##Resurface - - intro_server = IntrospectionServer(str(rospy.get_name()), robosub_state_machine,'/SM_ROOT') + intro_server = IntrospectionServer( + str(rospy.get_name()), robosub_state_machine, "/SM_ROOT" + ) intro_server.start() try: @@ -97,5 +107,5 @@ def main(): rospy.loginfo("State machine failed: %s" % e) -if __name__ == '__main__': - main() \ No newline at end of file +if __name__ == "__main__": + main() diff --git a/mission/finite_state_machine/scripts/simtest.py b/mission/finite_state_machine/scripts/simtest.py index d955685a3..9b34e6979 100755 --- a/mission/finite_state_machine/scripts/simtest.py +++ b/mission/finite_state_machine/scripts/simtest.py @@ -9,16 +9,13 @@ from smach_ros import IntrospectionServer -rospy.init_node('simtest_fsm') +rospy.init_node("simtest_fsm") -simtest_sm = patrol_sequence([ - dp_move(0, 0), - los_move(4, 0), - los_move(1, 0), - dp_move(0, 0, yaw_rad=pi) -]) +simtest_sm = patrol_sequence( + [dp_move(0, 0), los_move(4, 0), los_move(1, 0), dp_move(0, 0, yaw_rad=pi)] +) -intro_server = IntrospectionServer(str(rospy.get_name()), simtest_sm,'/SM_ROOT') +intro_server = IntrospectionServer(str(rospy.get_name()), simtest_sm, "/SM_ROOT") intro_server.start() @@ -27,4 +24,4 @@ intro_server.stop() except Exception as e: - rospy.loginfo("Pooltest failed: %s" % e) + rospy.loginfo("Pooltest failed: %s" % e) diff --git a/mission/finite_state_machine/scripts/simulator_state_machine.py b/mission/finite_state_machine/scripts/simulator_state_machine.py index 67c7e5d7d..31f3862b1 100755 --- a/mission/finite_state_machine/scripts/simulator_state_machine.py +++ b/mission/finite_state_machine/scripts/simulator_state_machine.py @@ -12,73 +12,94 @@ def main(): - rospy.init_node('simulator_state_machine') - - move_action_server = '/guidance/move' - - simulator_state_machine = StateMachine(outcomes=['preempted', 'succeeded', 'aborted']) - - - odom = Odometry(None,None,None,None) + rospy.init_node("simulator_state_machine") + + move_action_server = "/guidance/move" + + simulator_state_machine = StateMachine( + outcomes=["preempted", "succeeded", "aborted"] + ) + + odom = Odometry(None, None, None, None) + def odom_cb(odom_msg): odom = odom_msg - rospy.Subscriber("odometry/filtered", Odometry, odom_cb) - + + rospy.Subscriber("odometry/filtered", Odometry, odom_cb) + with simulator_state_machine: - - StateMachine.add('REACH_DEPTH', - dp_move(0,0), - transitions={'succeeded':'GATE_SM'}) - - gate_sm = StateMachine(outcomes=['preempted', 'succeeded', 'aborted']) - gate_sm.userdata.goal_position = Point(None,None,None) + StateMachine.add( + "REACH_DEPTH", dp_move(0, 0), transitions={"succeeded": "GATE_SM"} + ) + + gate_sm = StateMachine(outcomes=["preempted", "succeeded", "aborted"]) + + gate_sm.userdata.goal_position = Point(None, None, None) with gate_sm: - - StateMachine.add('GATE_SEARCH', - GateSearchState(), - transitions={'succeeded':'LOS_MOVE_TO_GATE'}, - remapping={'gate_search_output':'goal_position'}) - + + StateMachine.add( + "GATE_SEARCH", + GateSearchState(), + transitions={"succeeded": "LOS_MOVE_TO_GATE"}, + remapping={"gate_search_output": "goal_position"}, + ) + def gate_goal_cb(userdata, goal): gate_goal = MoveGoal() - gate_goal.target_pose.position = userdata.goal_position + gate_goal.target_pose.position = userdata.goal_position gate_goal.guidance_type = "LOS" return gate_goal - - StateMachine.add('LOS_MOVE_TO_GATE', - SimpleActionState(move_action_server, - MoveAction, - goal_cb=gate_goal_cb, - input_keys=['goal_position']), - transitions={'succeeded':'PREPARE_MOVE_THROUGH','aborted':'GATE_SEARCH'}, - remapping={'goal_position':'goal_position'}) - + + StateMachine.add( + "LOS_MOVE_TO_GATE", + SimpleActionState( + move_action_server, + MoveAction, + goal_cb=gate_goal_cb, + input_keys=["goal_position"], + ), + transitions={ + "succeeded": "PREPARE_MOVE_THROUGH", + "aborted": "GATE_SEARCH", + }, + remapping={"goal_position": "goal_position"}, + ) + def prep_goal_cb(userdata, goal): - prep_goal = MoveGoal() + prep_goal = MoveGoal() prep_goal.target_pose.position = userdata.goal_position x_dist = odom.pose.pose.position.x - userdata.goal_position.x - prep_goal.target_pose.position.x = userdata.goal_position.x + (x_dist/abs(x_dist))/2 - prep_goal.guidance_type = "PositionHold" + prep_goal.target_pose.position.x = ( + userdata.goal_position.x + (x_dist / abs(x_dist)) / 2 + ) + prep_goal.guidance_type = "PositionHold" return prep_goal - StateMachine.add('PREPARE_MOVE_THROUGH', - SimpleActionState(move_action_server, - MoveAction, - goal_cb=prep_goal_cb, - input_keys=['goal_position']), - transitions={'succeeded':'LOS_MOVE_THROUGH_GATE'}, - remapping={'goal_pos_input':'goal_position'}) - - StateMachine.add('LOS_MOVE_THROUGH_GATE', - los_move(8,1.5), - transitions={'aborted':'GATE_SEARCH'}) - - StateMachine.add('GATE_SM',gate_sm) - - - intro_server = IntrospectionServer(str(rospy.get_name()), simulator_state_machine,'/SM_ROOT') + StateMachine.add( + "PREPARE_MOVE_THROUGH", + SimpleActionState( + move_action_server, + MoveAction, + goal_cb=prep_goal_cb, + input_keys=["goal_position"], + ), + transitions={"succeeded": "LOS_MOVE_THROUGH_GATE"}, + remapping={"goal_pos_input": "goal_position"}, + ) + + StateMachine.add( + "LOS_MOVE_THROUGH_GATE", + los_move(8, 1.5), + transitions={"aborted": "GATE_SEARCH"}, + ) + + StateMachine.add("GATE_SM", gate_sm) + + intro_server = IntrospectionServer( + str(rospy.get_name()), simulator_state_machine, "/SM_ROOT" + ) intro_server.start() try: @@ -89,6 +110,5 @@ def prep_goal_cb(userdata, goal): rospy.loginfo("Pooltest failed: %s" % e) - -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/mission/finite_state_machine/scripts/sm_classes/__init__.py b/mission/finite_state_machine/scripts/sm_classes/__init__.py index 5ab6a9e74..cfee465ec 100644 --- a/mission/finite_state_machine/scripts/sm_classes/__init__.py +++ b/mission/finite_state_machine/scripts/sm_classes/__init__.py @@ -1,2 +1,2 @@ -#import common_states -#import helper \ No newline at end of file +# import common_states +# import helper diff --git a/mission/finite_state_machine/setup.py b/mission/finite_state_machine/setup.py index 903ded0e6..d8a61b1e0 100644 --- a/mission/finite_state_machine/setup.py +++ b/mission/finite_state_machine/setup.py @@ -5,4 +5,4 @@ setup_args = generate_distutils_setup( packages=["sm_classes", "si_sm_classes"], package_dir={"": "src"} ) -setup(**setup_args) \ No newline at end of file +setup(**setup_args) diff --git a/mission/finite_state_machine/src/finite_state_machine/helper.py b/mission/finite_state_machine/src/finite_state_machine/helper.py index 106e4c88d..e69027ba4 100644 --- a/mission/finite_state_machine/src/finite_state_machine/helper.py +++ b/mission/finite_state_machine/src/finite_state_machine/helper.py @@ -22,7 +22,6 @@ class ControlModeEnum(IntEnum): ORIENTATION_DEPTH_HOLD = 9 - def twist(X=0, Y=0, Z=0, K=0, M=0, N=0): new_twist = Twist() new_twist.linear.x = X diff --git a/mission/finite_state_machine/src/finite_state_machine/system_identification.py b/mission/finite_state_machine/src/finite_state_machine/system_identification.py index 7ca9b9621..0a4a2cbad 100755 --- a/mission/finite_state_machine/src/finite_state_machine/system_identification.py +++ b/mission/finite_state_machine/src/finite_state_machine/system_identification.py @@ -137,7 +137,7 @@ def __init__( goal_pose=None, timeout=20, goal_boundry=[0.5, 0.5, 0.2, 0.15, 0.15, 0.15], - end_pose=None + end_pose=None, ): State.__init__(self, outcomes=["preempted", "succeeded", "aborted"]) @@ -161,13 +161,13 @@ def __init__( ], goal_boundry=goal_boundry, odom_topic="/odometry/filtered", - ) + ), ] if not end_pose: states.append(GoToState(start_pose)) else: states.append(GoToState(end_pose)) - + names = ["go_to_start", "set_velocity", "monitor", "back_to_start"] self.sm = create_sequence(states, state_names=names) @@ -179,12 +179,13 @@ def surge_tests(): class SurgeTest(SingleTest): def __init__(self, velocity, orientation, end_pose=None): SingleTest.__init__( - self, + self, twist(X=velocity), pose(-1.5, 0, 0.7, 0, 0, orientation), dp_mode=ControlModeEnum.ORIENTATION_DEPTH_HOLD, - end_pose=end_pose + end_pose=end_pose, ) + states = [ SurgeTest(0.05, 0), SurgeTest(0.10, 0), @@ -206,12 +207,13 @@ def sway_tests(): class SwayTest(SingleTest): def __init__(self, velocity, orientation, end_pose=None): SingleTest.__init__( - self, + self, twist(Y=velocity), pose(-1.5, 0, 0.7, 0, 0, orientation), dp_mode=ControlModeEnum.ORIENTATION_DEPTH_HOLD, - end_pose=end_pose + end_pose=end_pose, ) + states = [ SwayTest(0.05, -90), SwayTest(0.10, -90), @@ -283,8 +285,9 @@ def __init__(self, surge_vel, sway_vel, initial_angle, end_pose=None): pose(-1.5, 0, 0.7, 0, 0, initial_angle), timeout=20, dp_mode=ControlModeEnum.ORIENTATION_DEPTH_HOLD, - end_pose=end_pose + end_pose=end_pose, ) + states = [ SurgeSwayTest(0.05, 0.05, -45), SurgeSwayTest(0.10, 0.10, -45), @@ -323,9 +326,9 @@ def __init__(self, velocity, start_depth, end_pose=None): twist(0, 0, velocity, 0, 0, 0), pose(0, 0, start_depth, 0, 0, 0), dp_mode=ControlModeEnum.ORIENTATION_HOLD, - end_pose=end_pose + end_pose=end_pose, ) - + states = [ HeaveTest(0.05, 0.5), HeaveTest(0.10, 0.5), @@ -339,15 +342,17 @@ def __init__(self, velocity, start_depth, end_pose=None): def heave_surge_tests(): class HeaveSurgeTest(SingleTest): - def __init__(self, heave_vel, surge_vel, initial_angle, initial_depth, end_pose=None): + def __init__( + self, heave_vel, surge_vel, initial_angle, initial_depth, end_pose=None + ): SingleTest.__init__( self, twist(surge_vel, 0, heave_vel, 0, 0, 0), pose(-1.5, 0, initial_depth, 0, 0, initial_angle), dp_mode=ControlModeEnum.ORIENTATION_HOLD, - end_pose=end_pose + end_pose=end_pose, ) - + states = [ HeaveSurgeTest(0.05, 0.1, 0, 0.5), HeaveSurgeTest(0.05, 0.2, 0, 0.5), @@ -357,16 +362,16 @@ def __init__(self, heave_vel, surge_vel, initial_angle, initial_depth, end_pose= HeaveSurgeTest(0.15, 0.2, 0, 0.5, end_pose=HOME_POSE), HeaveSurgeTest(-0.05, 0.1, 0, 1.0), HeaveSurgeTest(-0.05, 0.2, 0, 1.0), - HeaveSurgeTest(-0.1, 0.1, 0, 1.0), - HeaveSurgeTest(-0.1, 0.2, 0, 1.0), + HeaveSurgeTest(-0.1, 0.1, 0, 1.0), + HeaveSurgeTest(-0.1, 0.2, 0, 1.0), HeaveSurgeTest(-0.15, 0.1, 0, 1.0), HeaveSurgeTest(-0.15, 0.2, 0, 1.0, end_pose=HOME_POSE), - HeaveSurgeTest( 0.05, -0.1, 180, 0.5), - HeaveSurgeTest( 0.05, -0.2, 180, 0.5), - HeaveSurgeTest( 0.10, -0.1, 180, 0.5), - HeaveSurgeTest( 0.10, -0.2, 180, 0.5), - HeaveSurgeTest( 0.15, -0.1, 180, 0.5), - HeaveSurgeTest( 0.15, -0.2, 180, 0.5, end_pose=HOME_POSE), + HeaveSurgeTest(0.05, -0.1, 180, 0.5), + HeaveSurgeTest(0.05, -0.2, 180, 0.5), + HeaveSurgeTest(0.10, -0.1, 180, 0.5), + HeaveSurgeTest(0.10, -0.2, 180, 0.5), + HeaveSurgeTest(0.15, -0.1, 180, 0.5), + HeaveSurgeTest(0.15, -0.2, 180, 0.5, end_pose=HOME_POSE), HeaveSurgeTest(-0.05, -0.1, 180, 1.0), HeaveSurgeTest(-0.05, -0.2, 180, 1.0), HeaveSurgeTest(-0.10, -0.1, 180, 1.0), @@ -379,9 +384,9 @@ def __init__(self, heave_vel, surge_vel, initial_angle, initial_depth, end_pose= if __name__ == "__main__": rospy.init_node("system_identification_sm") - + HOME_POSE = pose(0, 0, 0.7, 0, 0, 0) states = yaw_tests() - + sm = create_sequence(states) sm.execute() diff --git a/mission/internal_status/bootscripts/display_battery_IP.py b/mission/internal_status/bootscripts/display_battery_IP.py index 7bc974505..edc01e47e 100644 --- a/mission/internal_status/bootscripts/display_battery_IP.py +++ b/mission/internal_status/bootscripts/display_battery_IP.py @@ -6,27 +6,29 @@ import subprocess import smbus -disp = Adafruit_SSD1306.SSD1306_128_64(rst=None, i2c_bus=1, gpio=1) #pins 27,28 equal i2c bus 1 +disp = Adafruit_SSD1306.SSD1306_128_64( + rst=None, i2c_bus=1, gpio=1 +) # pins 27,28 equal i2c bus 1 bus = smbus.SMBus(8) disp.begin() -#Clear display +# Clear display disp.clear() disp.display() # Create blank image width = disp.width height = disp.height -image = Image.new('1', (width, height)) +image = Image.new("1", (width, height)) draw = ImageDraw.Draw(image) -draw.rectangle((0,0,width,height), outline=0, fill=0) +draw.rectangle((0, 0, width, height), outline=0, fill=0) padding = -2 top = padding -bottom = height-padding +bottom = height - padding x = 0 @@ -35,29 +37,36 @@ while True: # Draw a black filled box to clear the image. - draw.rectangle((0,0,width,height), outline=0, fill=0) + draw.rectangle((0, 0, width, height), outline=0, fill=0) # Shell scripts for getting IP - cmd = "hostname -I | cut -d\' \' -f1" - IP = subprocess.check_output(cmd, shell = True ) - - xavier_mV = int(subprocess.check_output(["cat", "/sys/bus/i2c/drivers/ina3221x/1-0040/iio:device0/in_voltage0_input"]).decode("utf-8")) - xavier_V =str(xavier_mV / 1000.0) - - system_V = bus.read_i2c_block_data(0x6a, 0x00, 2) + cmd = "hostname -I | cut -d' ' -f1" + IP = subprocess.check_output(cmd, shell=True) + + xavier_mV = int( + subprocess.check_output( + [ + "cat", + "/sys/bus/i2c/drivers/ina3221x/1-0040/iio:device0/in_voltage0_input", + ] + ).decode("utf-8") + ) + xavier_V = str(xavier_mV / 1000.0) + + system_V = bus.read_i2c_block_data(0x6A, 0x00, 2) raw_adc_voltage = (system_V[0] & 0x0F) * 256 + system_V[1] system_V = str(raw_adc_voltage * 0.011) - #Script for getting SSH + # Script for getting SSH cmd = "w -h | wc -l" - n_of_users = subprocess.check_output(cmd, shell = True ) + n_of_users = subprocess.check_output(cmd, shell=True) - draw.text((x+2, top+3), "IP: " + IP, font=font, fill=255) - draw.text((x+2, top + 12),"Xavier: " + xavier_V + "v", font=font, fill = 255) - draw.text((x+2, top + 20),"System: " + system_V +"v", font=font, fill = 255) - draw.text((x+2, top + 40),"No. users: " + n_of_users, font=font, fill = 255) + draw.text((x + 2, top + 3), "IP: " + IP, font=font, fill=255) + draw.text((x + 2, top + 12), "Xavier: " + xavier_V + "v", font=font, fill=255) + draw.text((x + 2, top + 20), "System: " + system_V + "v", font=font, fill=255) + draw.text((x + 2, top + 40), "No. users: " + n_of_users, font=font, fill=255) # Display image. disp.image(image) disp.display() - time.sleep(.1) + time.sleep(0.1) diff --git a/mission/internal_status/src/battery_monitor.py b/mission/internal_status/src/battery_monitor.py index 8ee0bdf6e..30d050f53 100755 --- a/mission/internal_status/src/battery_monitor.py +++ b/mission/internal_status/src/battery_monitor.py @@ -10,172 +10,201 @@ class BatteryMonitor: - def __init__(self): - - rospy.init_node("battery_monitor") - - # Parameters - self.path_to_xavier_measurement = rospy.get_param("/battery/xavier/path", default="/sys/bus/i2c/drivers/ina3221x/1-0040/iio:device0/in_voltage0_input") - self.path_to_powersense = rospy.get_param("/battery/system/path", default="/dev/i2c-8") - - self.i2c_address_powersense_voltage = rospy.get_param("/i2c/psm/address_voltage", default=0x6a) - self.i2c_address_powersense_current = rospy.get_param("/i2c/psm/address_current", default=0x69) - self.i2c_bus_number = rospy.get_param("/i2c/psm/bus_number", default=8) - rospy.loginfo("PSM Voltage I2C address: '0x{:02x}'".format(self.i2c_address_powersense_voltage)) - rospy.loginfo("PSM Current I2C address: '0x{:02x}'".format(self.i2c_address_powersense_current)) - rospy.loginfo("PSM I2C bus number: '%d'", self.i2c_bus_number) - - self.critical_level = rospy.get_param("/battery/thresholds/critical", default=13.5) - self.warning_level = rospy.get_param("/battery/thresholds/warning", default=14.5) - - system_interval = rospy.get_param("/battery/system/interval", 0.05) - xavier_interval = rospy.get_param("/battery/xavier/interval", 10) - logging_interval = rospy.get_param("/battery/logging/interval", 10) - - # Local variables - self.xavier_voltage = 0.0 - self.system_voltage = 0.0 - self.system_current = 0.0 - - self.xavier_recieved = False - self.system_recieved = False - - self.is_PSM_fuckd_voltage = False - self.is_PSM_fuckd_current = False - - # Get I2C bus for power sense module - self.bus = smbus.SMBus(self.i2c_bus_number) - time.sleep(1) - - # Send configure command to the module to enable continuous conversion in 12-bit mode - try: - self.bus.write_byte(self.i2c_address_powersense_voltage, 0x10) - time.sleep(0.5) - except IOError: - print(traceback.format_exc()) - self.is_PSM_fuckd_voltage = True - - try: - self.bus.write_byte(self.i2c_address_powersense_current, 0x10) - time.sleep(0.5) - except IOError: - print(traceback.format_exc()) - self.is_PSM_fuckd_current = True - - - # Publishers - self.xavier_battery_level_pub = rospy.Publisher( - "/auv/battery_level/xavier", Float32, queue_size=1 - ) - if not self.is_PSM_fuckd_voltage: - self.system_battery_level_pub = rospy.Publisher( - "/auv/battery_level/system", Float32, queue_size=1 - ) - if not self.is_PSM_fuckd_current: - self.system_battery_current_draw_pub = rospy.Publisher( - "/auv/battery_level/system_current_draw", Float32, queue_size=1 - ) - - # set up callbacks - - self.xavier_timer = rospy.Timer(rospy.Duration(secs=xavier_interval), self.xavier_cb) - self.log_timer = rospy.Timer(rospy.Duration(secs=logging_interval), self.log_cb) - - if not self.is_PSM_fuckd_voltage or not self.is_PSM_fuckd_current: - self.system_timer = rospy.Timer(rospy.Duration(secs=system_interval), self.system_cb) - rospy.loginfo("BatteryMonitor initialized") - else: - rospy.logwarn("System Battery Monitoring from PSM is not available.") - - def xavier_cb(self, event): - """Record output from voltage meter command, decode from bytes object to string, convert from string to integer""" - xavier_mV = int( - subprocess.check_output(["cat", self.path_to_xavier_measurement]).decode("utf-8") - ) - self.xavier_voltage = xavier_mV / 1000.0 - - self.xavier_battery_level_pub.publish(self.xavier_voltage) - if not self.xavier_recieved: - self.xavier_recieved = True - - def system_cb(self, event): - """Read voltage of system from powersense device.""" - - # MCP3425 address, 0x68(104) - # Read data back from 0x00(00), 2 bytes, MSB first - # raw_adc MSB, raw_adc LSB - if not self.is_PSM_fuckd_voltage: - voltage_msg = self.bus.read_i2c_block_data(self.i2c_address_powersense_voltage, 0x00, 2) - - # Convert the data to 12-bits - raw_adc_voltage = (voltage_msg[0] & 0x0F) * 256 + voltage_msg[1] - if raw_adc_voltage > 2047: - raw_adc_voltage -= 4095 - - # MCP3425 address, 0x68(104) - # Read data back from 0x00(00), 2 bytes, MSB first - # raw_adc MSB, raw_adc LSB - if not self.is_PSM_fuckd_current: - current_msg = self.bus.read_i2c_block_data(self.i2c_address_powersense_current, 0x00, 2) - # Convert the data to 12-bits - raw_adc_current = (current_msg[0] & 0x0F) * 256 + current_msg[1] - if raw_adc_current > 2047: - raw_adc_current -= 4095 - - # PSM specific conversion ratio - if not self.is_PSM_fuckd_voltage: - self.system_voltage = raw_adc_voltage * 0.011 - if not self.is_PSM_fuckd_current: - self.system_current = raw_adc_current * 0.0504 - - # publish - if not self.is_PSM_fuckd_voltage: - self.system_battery_level_pub.publish(self.system_voltage) - if not self.is_PSM_fuckd_current: - self.system_battery_current_draw_pub.publish(self.system_current) - if not self.system_recieved: - self.system_recieved = True - - def log_cb(self, event): - - if self.xavier_recieved: - self.log_voltage(self.xavier_voltage, "xavier") - else: - rospy.loginfo("No voltage recieved from Xavier yet.") - - if self.system_recieved: - self.log_voltage(self.system_voltage, "system") - else: - rospy.loginfo("No voltage recieved from system yet.") - - - def log_voltage(self, voltage, title): - - if voltage == 0: - rospy.loginfo("Voltage is zero. Killswitch is probably off.") - - # Critical voltage level - elif voltage <= self.critical_level: - rospy.logerr("Critical %s voltage: %.3fV" % (title, voltage)) - - # Warning voltage level - elif voltage <= self.warning_level: - rospy.logwarn("%s voltage: %.3fV" % (title, voltage)) - - else: - rospy.loginfo("%s voltage: %.3fV" % (title, voltage)) - - def shutdown(self): - if not self.is_PSM_fuckd_voltage or not self.is_PSM_fuckd_current: - self.system_timer.shutdown() - self.xavier_timer.shutdown() - self.log_timer.shutdown() - self.bus.close() + def __init__(self): + + rospy.init_node("battery_monitor") + + # Parameters + self.path_to_xavier_measurement = rospy.get_param( + "/battery/xavier/path", + default="/sys/bus/i2c/drivers/ina3221x/1-0040/iio:device0/in_voltage0_input", + ) + self.path_to_powersense = rospy.get_param( + "/battery/system/path", default="/dev/i2c-8" + ) + + self.i2c_address_powersense_voltage = rospy.get_param( + "/i2c/psm/address_voltage", default=0x6A + ) + self.i2c_address_powersense_current = rospy.get_param( + "/i2c/psm/address_current", default=0x69 + ) + self.i2c_bus_number = rospy.get_param("/i2c/psm/bus_number", default=8) + rospy.loginfo( + "PSM Voltage I2C address: '0x{:02x}'".format( + self.i2c_address_powersense_voltage + ) + ) + rospy.loginfo( + "PSM Current I2C address: '0x{:02x}'".format( + self.i2c_address_powersense_current + ) + ) + rospy.loginfo("PSM I2C bus number: '%d'", self.i2c_bus_number) + + self.critical_level = rospy.get_param( + "/battery/thresholds/critical", default=13.5 + ) + self.warning_level = rospy.get_param( + "/battery/thresholds/warning", default=14.5 + ) + + system_interval = rospy.get_param("/battery/system/interval", 0.05) + xavier_interval = rospy.get_param("/battery/xavier/interval", 10) + logging_interval = rospy.get_param("/battery/logging/interval", 10) + + # Local variables + self.xavier_voltage = 0.0 + self.system_voltage = 0.0 + self.system_current = 0.0 + + self.xavier_recieved = False + self.system_recieved = False + + self.is_PSM_fuckd_voltage = False + self.is_PSM_fuckd_current = False + + # Get I2C bus for power sense module + self.bus = smbus.SMBus(self.i2c_bus_number) + time.sleep(1) + + # Send configure command to the module to enable continuous conversion in 12-bit mode + try: + self.bus.write_byte(self.i2c_address_powersense_voltage, 0x10) + time.sleep(0.5) + except IOError: + print(traceback.format_exc()) + self.is_PSM_fuckd_voltage = True + + try: + self.bus.write_byte(self.i2c_address_powersense_current, 0x10) + time.sleep(0.5) + except IOError: + print(traceback.format_exc()) + self.is_PSM_fuckd_current = True + + # Publishers + self.xavier_battery_level_pub = rospy.Publisher( + "/auv/battery_level/xavier", Float32, queue_size=1 + ) + if not self.is_PSM_fuckd_voltage: + self.system_battery_level_pub = rospy.Publisher( + "/auv/battery_level/system", Float32, queue_size=1 + ) + if not self.is_PSM_fuckd_current: + self.system_battery_current_draw_pub = rospy.Publisher( + "/auv/battery_level/system_current_draw", Float32, queue_size=1 + ) + + # set up callbacks + + self.xavier_timer = rospy.Timer( + rospy.Duration(secs=xavier_interval), self.xavier_cb + ) + self.log_timer = rospy.Timer(rospy.Duration(secs=logging_interval), self.log_cb) + + if not self.is_PSM_fuckd_voltage or not self.is_PSM_fuckd_current: + self.system_timer = rospy.Timer( + rospy.Duration(secs=system_interval), self.system_cb + ) + rospy.loginfo("BatteryMonitor initialized") + else: + rospy.logwarn("System Battery Monitoring from PSM is not available.") + + def xavier_cb(self, event): + """Record output from voltage meter command, decode from bytes object to string, convert from string to integer""" + xavier_mV = int( + subprocess.check_output(["cat", self.path_to_xavier_measurement]).decode( + "utf-8" + ) + ) + self.xavier_voltage = xavier_mV / 1000.0 + + self.xavier_battery_level_pub.publish(self.xavier_voltage) + if not self.xavier_recieved: + self.xavier_recieved = True + + def system_cb(self, event): + """Read voltage of system from powersense device.""" + + # MCP3425 address, 0x68(104) + # Read data back from 0x00(00), 2 bytes, MSB first + # raw_adc MSB, raw_adc LSB + if not self.is_PSM_fuckd_voltage: + voltage_msg = self.bus.read_i2c_block_data( + self.i2c_address_powersense_voltage, 0x00, 2 + ) + + # Convert the data to 12-bits + raw_adc_voltage = (voltage_msg[0] & 0x0F) * 256 + voltage_msg[1] + if raw_adc_voltage > 2047: + raw_adc_voltage -= 4095 + + # MCP3425 address, 0x68(104) + # Read data back from 0x00(00), 2 bytes, MSB first + # raw_adc MSB, raw_adc LSB + if not self.is_PSM_fuckd_current: + current_msg = self.bus.read_i2c_block_data( + self.i2c_address_powersense_current, 0x00, 2 + ) + # Convert the data to 12-bits + raw_adc_current = (current_msg[0] & 0x0F) * 256 + current_msg[1] + if raw_adc_current > 2047: + raw_adc_current -= 4095 + + # PSM specific conversion ratio + if not self.is_PSM_fuckd_voltage: + self.system_voltage = raw_adc_voltage * 0.011 + if not self.is_PSM_fuckd_current: + self.system_current = raw_adc_current * 0.0504 + + # publish + if not self.is_PSM_fuckd_voltage: + self.system_battery_level_pub.publish(self.system_voltage) + if not self.is_PSM_fuckd_current: + self.system_battery_current_draw_pub.publish(self.system_current) + if not self.system_recieved: + self.system_recieved = True + + def log_cb(self, event): + + if self.xavier_recieved: + self.log_voltage(self.xavier_voltage, "xavier") + else: + rospy.loginfo("No voltage recieved from Xavier yet.") + + if self.system_recieved: + self.log_voltage(self.system_voltage, "system") + else: + rospy.loginfo("No voltage recieved from system yet.") + + def log_voltage(self, voltage, title): + + if voltage == 0: + rospy.loginfo("Voltage is zero. Killswitch is probably off.") + + # Critical voltage level + elif voltage <= self.critical_level: + rospy.logerr("Critical %s voltage: %.3fV" % (title, voltage)) + + # Warning voltage level + elif voltage <= self.warning_level: + rospy.logwarn("%s voltage: %.3fV" % (title, voltage)) + + else: + rospy.loginfo("%s voltage: %.3fV" % (title, voltage)) + + def shutdown(self): + if not self.is_PSM_fuckd_voltage or not self.is_PSM_fuckd_current: + self.system_timer.shutdown() + self.xavier_timer.shutdown() + self.log_timer.shutdown() + self.bus.close() if __name__ == "__main__": - bm = BatteryMonitor() - try: - rospy.spin() - finally: - bm.shutdown() + bm = BatteryMonitor() + try: + rospy.spin() + finally: + bm.shutdown() diff --git a/mission/internal_status/src/temperature_monitor.py b/mission/internal_status/src/temperature_monitor.py index 26d3b7e03..9fc1d0a33 100755 --- a/mission/internal_status/src/temperature_monitor.py +++ b/mission/internal_status/src/temperature_monitor.py @@ -1,55 +1,64 @@ #!/usr/bin/python3 -#python imports +# python imports import subprocess import re -#ros imports +# ros imports import rospy from std_msgs.msg import Int32 -class TemperatureMonitor(): - def __init__(self): - - rospy.init_node("temperature_monitor") - # Settings - self.interval = rospy.get_param("/temperature/logging/interval") # How often the battery level is checked and published - self.temperature_template = r"\d*\.{0,1}\d*" # In python file because yaml doesn't like escape characters - - # Publisher - self.cpu_temperature_pub = rospy.Publisher("/auv/temperature/cpu", Int32, queue_size=1) - self.gpu_temperature_pub = rospy.Publisher("/auv/temperature/gpu", Int32, queue_size=1) - - # Start the tegrastats subprocess - self.process = subprocess.Popen("tegrastats", shell=False, stdout=subprocess.PIPE) +class TemperatureMonitor: + def __init__(self): - def measure_temp(self): + rospy.init_node("temperature_monitor") - # Record output from temperature meter command, decode from bytes object to string, convert from string to integer + # Settings + self.interval = rospy.get_param( + "/temperature/logging/interval" + ) # How often the battery level is checked and published + self.temperature_template = r"\d*\.{0,1}\d*" # In python file because yaml doesn't like escape characters - stats = self.process.stdout.readlines()[-1].decode("utf-8") + # Publisher + self.cpu_temperature_pub = rospy.Publisher( + "/auv/temperature/cpu", Int32, queue_size=1 + ) + self.gpu_temperature_pub = rospy.Publisher( + "/auv/temperature/gpu", Int32, queue_size=1 + ) - if stats != "": + # Start the tegrastats subprocess + self.process = subprocess.Popen( + "tegrastats", shell=False, stdout=subprocess.PIPE + ) - cpu_search = re.search("CPU@("+self.temperature_template+")C", stats) - gpu_search = re.search("GPU@("+self.temperature_template+")C", stats) + def measure_temp(self): - self.cpu_temperature = int(float(cpu_search.group(1))) - self.gpu_temperature = int(float(gpu_search.group(1))) + # Record output from temperature meter command, decode from bytes object to string, convert from string to integer + stats = self.process.stdout.readlines()[-1].decode("utf-8") - def spin(self): - # Main loop - while not rospy.is_shutdown(): + if stats != "": - self.measure_temp() - self.cpu_temperature_pub.publish(self.cpu_temperature) - self.gpu_temperature_pub.publish(self.gpu_temperature) + cpu_search = re.search("CPU@(" + self.temperature_template + ")C", stats) + gpu_search = re.search("GPU@(" + self.temperature_template + ")C", stats) + + self.cpu_temperature = int(float(cpu_search.group(1))) + self.gpu_temperature = int(float(gpu_search.group(1))) + + def spin(self): + # Main loop + while not rospy.is_shutdown(): + + self.measure_temp() + self.cpu_temperature_pub.publish(self.cpu_temperature) + self.gpu_temperature_pub.publish(self.gpu_temperature) + + rospy.sleep(self.interval) - rospy.sleep(self.interval) if __name__ == "__main__": - tm = TemperatureMonitor() - tm.spin() + tm = TemperatureMonitor() + tm.spin() diff --git a/mission/joystick_interface/scripts/joystick_interface.py b/mission/joystick_interface/scripts/joystick_interface.py index 8e127ea34..7c73a5e3c 100755 --- a/mission/joystick_interface/scripts/joystick_interface.py +++ b/mission/joystick_interface/scripts/joystick_interface.py @@ -9,6 +9,7 @@ from sensor_msgs.msg import Joy from vortex_msgs.msg import ControlModeAction, ControlModeGoal + class ControlModeEnum(IntEnum): OPEN_LOOP = 0 POSITION_HOLD = 1 @@ -21,43 +22,61 @@ class ControlModeEnum(IntEnum): ORIENTATION_HOLD = 8 ORIENTATION_DEPTH_HOLD = 9 -class JoystickInterface(): - +class JoystickInterface: def __init__(self): """ Define constants used in the joystick mapping, and any ros specifics """ - rospy.init_node('joystick_interface') - - self.joystick_surge_scaling = rospy.get_param('/joystick/scaling/surge') - self.joystick_sway_scaling = rospy.get_param('/joystick/scaling/sway') - self.joystick_heave_scaling = rospy.get_param('/joystick/scaling/heave') - self.joystick_roll_scaling = rospy.get_param('/joystick/scaling/roll') - self.joystick_pitch_scaling = rospy.get_param('/joystick/scaling/pitch') - self.joystick_yaw_scaling = rospy.get_param('/joystick/scaling/yaw') - - self.joystick_buttons_map = ['A', 'B', 'X', 'Y', 'LB', 'RB', 'back', - 'start', 'power', 'stick_button_left', - 'stick_button_right'] - - self.joystick_axes_map = ['horizontal_axis_left_stick', - 'vertical_axis_left_stick', 'LT', - 'horizontal_axis_right_stick', - 'vertical_axis_right_stick', 'RT', - 'dpad_horizontal', 'dpad_vertical'] - - - - self.joystick_sub = rospy.Subscriber('/joystick/joy', Joy, self.joystick_cb, queue_size=1) - self.joystick_pub = rospy.Publisher('/mission/joystick_data', Joy, queue_size=1) - self.control_mode_pub = rospy.Publisher('/mission/control_mode', String, queue_size=1) - - self.guidance_interface_client = actionlib.SimpleActionClient("/guidance_interface/joystick_server", ControlModeAction) - - rospy.loginfo('Joystick interface is up and running') + rospy.init_node("joystick_interface") + + self.joystick_surge_scaling = rospy.get_param("/joystick/scaling/surge") + self.joystick_sway_scaling = rospy.get_param("/joystick/scaling/sway") + self.joystick_heave_scaling = rospy.get_param("/joystick/scaling/heave") + self.joystick_roll_scaling = rospy.get_param("/joystick/scaling/roll") + self.joystick_pitch_scaling = rospy.get_param("/joystick/scaling/pitch") + self.joystick_yaw_scaling = rospy.get_param("/joystick/scaling/yaw") + + self.joystick_buttons_map = [ + "A", + "B", + "X", + "Y", + "LB", + "RB", + "back", + "start", + "power", + "stick_button_left", + "stick_button_right", + ] + + self.joystick_axes_map = [ + "horizontal_axis_left_stick", + "vertical_axis_left_stick", + "LT", + "horizontal_axis_right_stick", + "vertical_axis_right_stick", + "RT", + "dpad_horizontal", + "dpad_vertical", + ] + + self.joystick_sub = rospy.Subscriber( + "/joystick/joy", Joy, self.joystick_cb, queue_size=1 + ) + self.joystick_pub = rospy.Publisher("/mission/joystick_data", Joy, queue_size=1) + self.control_mode_pub = rospy.Publisher( + "/mission/control_mode", String, queue_size=1 + ) + + self.guidance_interface_client = actionlib.SimpleActionClient( + "/guidance_interface/joystick_server", ControlModeAction + ) + + rospy.loginfo("Joystick interface is up and running") def joystick_cb(self, msg): buttons = {} @@ -69,7 +88,6 @@ def joystick_cb(self, msg): for i in range(len(msg.axes)): axes[self.joystick_axes_map[i]] = msg.axes[i] - abxy = self._abxy_pressed(buttons) if abxy != -1: @@ -78,7 +96,7 @@ def joystick_cb(self, msg): cm = ControlModeGoal() cm.controlModeIndex = abxy self.guidance_interface_client.send_goal(cm) - + control_mode_msg = String() if abxy == 0: control_mode_msg.data = "open loop" @@ -88,49 +106,59 @@ def joystick_cb(self, msg): control_mode_msg.data = "depth heading hold" elif abxy == 1: control_mode_msg.data = "position hold" - else: + else: control_mode_msg.data = "unknown control mode" self.control_mode_pub.publish(control_mode_msg) - - rospy.sleep(rospy.Duration(0.25)) # Sleep to avoid aggressie switching - - surge = axes['vertical_axis_left_stick'] * self.joystick_surge_scaling - sway = axes['horizontal_axis_left_stick'] * self.joystick_sway_scaling - heave = (axes['RT'] - axes['LT'])/2 * self.joystick_heave_scaling - roll = (buttons['RB'] - buttons['LB']) * self.joystick_roll_scaling - pitch = axes['vertical_axis_right_stick'] * self.joystick_pitch_scaling * (-1) - yaw = axes['horizontal_axis_right_stick'] * self.joystick_yaw_scaling - - dpad_lights = axes['dpad_horizontal'] - dpad_gripper = axes['dpad_vertical'] + + rospy.sleep(rospy.Duration(0.25)) # Sleep to avoid aggressie switching + + surge = axes["vertical_axis_left_stick"] * self.joystick_surge_scaling + sway = axes["horizontal_axis_left_stick"] * self.joystick_sway_scaling + heave = (axes["RT"] - axes["LT"]) / 2 * self.joystick_heave_scaling + roll = (buttons["RB"] - buttons["LB"]) * self.joystick_roll_scaling + pitch = axes["vertical_axis_right_stick"] * self.joystick_pitch_scaling * (-1) + yaw = axes["horizontal_axis_right_stick"] * self.joystick_yaw_scaling + + dpad_lights = axes["dpad_horizontal"] + dpad_gripper = axes["dpad_vertical"] joystick_msg = Joy() - joystick_msg.axes = [surge, sway, heave, roll, pitch, yaw, dpad_lights, dpad_gripper] - + joystick_msg.axes = [ + surge, + sway, + heave, + roll, + pitch, + yaw, + dpad_lights, + dpad_gripper, + ] + self.joystick_pub.publish(joystick_msg) def _abxy_pressed(self, buttons): pressed = -1 - if buttons['A']: + if buttons["A"]: pressed = ControlModeEnum.OPEN_LOOP.value - - if buttons['B']: + + if buttons["B"]: pressed = ControlModeEnum.ORIENTATION_DEPTH_HOLD.value - if buttons['X']: + if buttons["X"]: pressed = ControlModeEnum.DEPTH_HOLD.value - if buttons['Y']: + if buttons["Y"]: pressed = ControlModeEnum.POSITION_HEADING_HOLD.value return pressed -if __name__ == '__main__': - + +if __name__ == "__main__": + try: joystick_interface = JoystickInterface() rospy.spin() except rospy.ROSInterruptException: - pass \ No newline at end of file + pass diff --git a/mission/landmarks/include/landmarks/landmarks.h b/mission/landmarks/include/landmarks/landmarks.h index 2bd4bed80..311b6c16c 100644 --- a/mission/landmarks/include/landmarks/landmarks.h +++ b/mission/landmarks/include/landmarks/landmarks.h @@ -1,42 +1,45 @@ -#include "ros/ros.h" #include "geometry_msgs/Point.h" #include "geometry_msgs/PoseStamped.h" -#include +#include "landmarks/request_position.h" +#include "ros/ros.h" #include #include -#include "landmarks/request_position.h" +#include -class Landmarks{ - /** - * To serve as an interface between the perception system and the control system, - * an instance of this class receives object positions ("landmarks") published by - * the perception system on a ROS-topic. The object positions are stored in a map - * before they are published on a ROS-topic which the control system is subscribed to. - */ +class Landmarks { + /** + * To serve as an interface between the perception system and the control + * system, an instance of this class receives object positions ("landmarks") + * published by the perception system on a ROS-topic. The object positions are + * stored in a map before they are published on a ROS-topic which the control + * system is subscribed to. + */ public: - Landmarks(); - /** - * The callback function for the op_sub-subscriber. - * @param objPose is the message received on the ROS-topic, containing an object ID and the position of the object. - * The object ID and position is stored in the objectPositions-map - * The message received is further published on the object_positions_out-topic. - */ - void callback(vortex_msgs::ObjectPosition objPose); - /** - * ros::spinOnce() is called at 10Hz - */ - void execute(); - /** - * Prints the contents of the objectPositions-map - */ - void printMap(std::map objectsMap); + Landmarks(); + /** + * The callback function for the op_sub-subscriber. + * @param objPose is the message received on the ROS-topic, containing an + * object ID and the position of the object. The object ID and position is + * stored in the objectPositions-map The message received is further published + * on the object_positions_out-topic. + */ + void callback(vortex_msgs::ObjectPosition objPose); + /** + * ros::spinOnce() is called at 10Hz + */ + void execute(); + /** + * Prints the contents of the objectPositions-map + */ + void printMap(std::map objectsMap); protected: - ros::NodeHandle n; - ros::Subscriber op_sub; - ros::Publisher op_pub; - ros::Rate loop_rate; - std::map objectPositions; - bool send_pos(landmarks::request_position::Request& req, landmarks::request_position::Response& res); - ros::ServiceServer service; + ros::NodeHandle n; + ros::Subscriber op_sub; + ros::Publisher op_pub; + ros::Rate loop_rate; + std::map objectPositions; + bool send_pos(landmarks::request_position::Request &req, + landmarks::request_position::Response &res); + ros::ServiceServer service; }; \ No newline at end of file diff --git a/mission/landmarks/src/landmarks.cpp b/mission/landmarks/src/landmarks.cpp index 200a93a63..681372096 100644 --- a/mission/landmarks/src/landmarks.cpp +++ b/mission/landmarks/src/landmarks.cpp @@ -1,48 +1,50 @@ #include "landmarks/landmarks.h" -Landmarks::Landmarks ():loop_rate(10) { - op_sub = n.subscribe("object_positions_in",10, &Landmarks::callback, this); - op_pub = n.advertise("object_positions_out",10); - service = n.advertiseService("send_positions", &Landmarks::send_pos, this); - geometry_msgs::Point p; - vortex_msgs::ObjectPosition obj; - obj.objectPose.pose.position = p; - obj.isDetected = false; - obj.estimateConverged = false; - obj.estimateFucked = false; - objectPositions["gate"] = obj; - objectPositions["pole"] = obj; - objectPositions["path"] = obj; - objectPositions["buoy"] = obj; - objectPositions["recovery_point"] = obj; +Landmarks::Landmarks() : loop_rate(10) { + op_sub = n.subscribe("object_positions_in", 10, &Landmarks::callback, this); + op_pub = n.advertise("object_positions_out", 10); + service = n.advertiseService("send_positions", &Landmarks::send_pos, this); + geometry_msgs::Point p; + vortex_msgs::ObjectPosition obj; + obj.objectPose.pose.position = p; + obj.isDetected = false; + obj.estimateConverged = false; + obj.estimateFucked = false; + objectPositions["gate"] = obj; + objectPositions["pole"] = obj; + objectPositions["path"] = obj; + objectPositions["buoy"] = obj; + objectPositions["recovery_point"] = obj; } -void Landmarks::callback(vortex_msgs::ObjectPosition objPose){ - objectPositions[objPose.objectID] = objPose; - op_pub.publish(objPose); +void Landmarks::callback(vortex_msgs::ObjectPosition objPose) { + objectPositions[objPose.objectID] = objPose; + op_pub.publish(objPose); } -void Landmarks::execute(){ - while (ros::ok()){ - ros::spinOnce(); - loop_rate.sleep(); - } +void Landmarks::execute() { + while (ros::ok()) { + ros::spinOnce(); + loop_rate.sleep(); + } } -void Landmarks::printMap(std::map objectsMap){ - for(auto elem : objectsMap){ - ROS_INFO("ID: %s", elem.first.c_str()); - ROS_INFO("position: %f,%f,%f",elem.second.x,elem.second.y,elem.second.z); - } +void Landmarks::printMap( + std::map objectsMap) { + for (auto elem : objectsMap) { + ROS_INFO("ID: %s", elem.first.c_str()); + ROS_INFO("position: %f,%f,%f", elem.second.x, elem.second.y, elem.second.z); + } } -bool Landmarks::send_pos(landmarks::request_position::Request &req, landmarks::request_position::Response &res){ - res.object= Landmarks::objectPositions[req.ID]; - return true; +bool Landmarks::send_pos(landmarks::request_position::Request &req, + landmarks::request_position::Response &res) { + res.object = Landmarks::objectPositions[req.ID]; + return true; } -int main(int argc, char **argv){ - ros::init(argc,argv,"landmarks"); - Landmarks lm; - lm.execute(); +int main(int argc, char **argv) { + ros::init(argc, argv, "landmarks"); + Landmarks lm; + lm.execute(); } \ No newline at end of file diff --git a/motion/dp_controller/include/dp_controller/control_modes.h b/motion/dp_controller/include/dp_controller/control_modes.h index 6aad3d5d2..559103a1f 100644 --- a/motion/dp_controller/include/dp_controller/control_modes.h +++ b/motion/dp_controller/include/dp_controller/control_modes.h @@ -7,10 +7,8 @@ #include -namespace ControlModes -{ -enum ControlMode -{ +namespace ControlModes { +enum ControlMode { OPEN_LOOP = 0, POSITION_HOLD = 1, HEADING_HOLD = 2, @@ -22,7 +20,7 @@ enum ControlMode ORIENTATION_HOLD = 8, ORIENTATION_DEPTH_HOLD = 9 }; -} // namespace ControlModes +} // namespace ControlModes typedef ControlModes::ControlMode ControlMode; /** @@ -31,52 +29,50 @@ typedef ControlModes::ControlMode ControlMode; * @param control_mode the control mode to be converted to a string * */ -inline std::string controlModeString(ControlMode control_mode) -{ +inline std::string controlModeString(ControlMode control_mode) { std::string s; - switch (control_mode) - { - case ControlModes::OPEN_LOOP: - s = "OPEN LOOP"; - break; + switch (control_mode) { + case ControlModes::OPEN_LOOP: + s = "OPEN LOOP"; + break; - case ControlModes::POSITION_HOLD: - s = "POSITION HOLD"; - break; + case ControlModes::POSITION_HOLD: + s = "POSITION HOLD"; + break; - case ControlModes::HEADING_HOLD: - s = "HEADING HOLD"; - break; + case ControlModes::HEADING_HOLD: + s = "HEADING HOLD"; + break; - case ControlModes::DEPTH_HEADING_HOLD: - s = "DEPTH HEADING HOLD"; - break; + case ControlModes::DEPTH_HEADING_HOLD: + s = "DEPTH HEADING HOLD"; + break; - case ControlModes::DEPTH_HOLD: - s = "DEPTH HOLD"; - break; + case ControlModes::DEPTH_HOLD: + s = "DEPTH HOLD"; + break; - case ControlModes::POSITION_HEADING_HOLD: - s = "POSITION HEADING HOLD"; - break; - - case ControlModes::POSE_HOLD: - s = "POSE HOLD"; - break; + case ControlModes::POSITION_HEADING_HOLD: + s = "POSITION HEADING HOLD"; + break; - case ControlModes::ORIENTATION_HOLD: - s = "ORIENTATION HOLD"; - break; + case ControlModes::POSE_HOLD: + s = "POSE HOLD"; + break; - case ControlModes::ORIENTATION_DEPTH_HOLD: - s = "ORIENTATION DEPTH HOLD"; - break; + case ControlModes::ORIENTATION_HOLD: + s = "ORIENTATION HOLD"; + break; - default: - s = "INVALID CONTROL MODE"; - break; + case ControlModes::ORIENTATION_DEPTH_HOLD: + s = "ORIENTATION DEPTH HOLD"; + break; + + default: + s = "INVALID CONTROL MODE"; + break; } return s; } -#endif // VORTEX_CONTROLLER_CONTROL_MODES_H +#endif // VORTEX_CONTROLLER_CONTROL_MODES_H diff --git a/motion/dp_controller/include/dp_controller/controller_ros.h b/motion/dp_controller/include/dp_controller/controller_ros.h index 4fd1aa209..e49361567 100644 --- a/motion/dp_controller/include/dp_controller/controller_ros.h +++ b/motion/dp_controller/include/dp_controller/controller_ros.h @@ -12,39 +12,40 @@ #ifndef VORTEX_CONTROLLER_CONTROLLER_ROS_H #define VORTEX_CONTROLLER_CONTROLLER_ROS_H -#include #include +#include #include #include -#include -#include -#include +#include #include -#include +#include #include -#include #include +#include +#include #include +#include #include #include -#include "vortex_msgs/Debug.h" #include "vortex_msgs/ControlMode.h" +#include "vortex_msgs/Debug.h" +#include "vortex_msgs/DpSetpoint.h" #include "vortex_msgs/PropulsionCommand.h" #include "vortex_msgs/RovState.h" -#include "vortex_msgs/DpSetpoint.h" -#include "dp_controller/quaternion_pd_controller.h" #include "dp_controller/VortexControllerConfig.h" -#include "dp_controller/state.h" -#include "dp_controller/setpoints.h" #include "dp_controller/eigen_helper.h" #include "dp_controller/eigen_typedefs.h" +#include "dp_controller/quaternion_pd_controller.h" +#include "dp_controller/setpoints.h" +#include "dp_controller/state.h" // typedef so you dont have to write out definition every time -typedef actionlib::SimpleActionServer MoveBaseActionServer; +typedef actionlib::SimpleActionServer + MoveBaseActionServer; /** * @brief the Controller class @@ -53,8 +54,7 @@ typedef actionlib::SimpleActionServer MoveBaseAc * @see quaternion_pd_controller.h * */ -class Controller -{ +class Controller { public: /** * @brief Controller class constructor @@ -69,14 +69,15 @@ class Controller * If the orientation given in @p msg is invalid, this function returns early. * Else it publishes the calculated feedback through the action server. * - * @param msg A nav_msg::Odometry message containing state data about the AUV. + * @param msg A nav_msg::Odometry message containing state data about the + * AUV. */ - void stateCallback(const nav_msgs::Odometry& msg); + void stateCallback(const nav_msgs::Odometry &msg); /** * @brief Callback for the reference model subscriber */ - void guidanceCallback(const vortex_msgs::DpSetpoint& msg); + void guidanceCallback(const vortex_msgs::DpSetpoint &msg); /** * @brief Callback for the dynamic reconfigure server @@ -87,7 +88,8 @@ class Controller * This function sets the controller gains for the @p config and passes these * to the @c setGains() command in the controller. */ - void configCallback(const dp_controller::VortexControllerConfig& config, uint32_t level); + void configCallback(const dp_controller::VortexControllerConfig &config, + uint32_t level); // /** // * @brief Action server; goal @@ -122,12 +124,15 @@ class Controller ros::Publisher m_mode_pub; /** Mode publisher */ ros::Publisher m_debug_pub; /** Debug publisher */ - dynamic_reconfigure::Server m_dr_srv; /** dynamic_reconfigure server */ + dynamic_reconfigure::Server + m_dr_srv; /** dynamic_reconfigure server */ - ControlMode prev_control_mode; /** Previous control mode */ - bool m_debug_mode = false; /** Bool to run in debug mode */ - const double c_normalized_force_deadzone = 0.01; /** Normalized force deadzone */ - const double c_max_quat_norm_deviation = 0.1; /** Maximum normalized deviation (quaternion) */ + ControlMode prev_control_mode; /** Previous control mode */ + bool m_debug_mode = false; /** Bool to run in debug mode */ + const double c_normalized_force_deadzone = + 0.01; /** Normalized force deadzone */ + const double c_max_quat_norm_deviation = + 0.1; /** Maximum normalized deviation (quaternion) */ // Internal state Eigen::Vector3d m_current_position; @@ -144,8 +149,7 @@ class Controller Eigen::Quaterniond orientation; /** Current orientation */ Eigen::Vector6d velocity; /** Current velocity */ - enum PoseIndex - { + enum PoseIndex { SURGE = 0, SWAY = 1, HEAVE = 2, @@ -153,12 +157,7 @@ class Controller PITCH = 4, YAW = 5 }; - enum EulerIndex - { - EULER_YAW = 0, - EULER_PITCH = 1, - EULER_ROLL = 2 - }; + enum EulerIndex { EULER_YAW = 0, EULER_PITCH = 1, EULER_ROLL = 2 }; /** * @brief Convert integer to ControlMode @@ -195,136 +194,175 @@ class Controller void initPositionHoldController(); /** - * @brief Publish a vortex_msgs Debug message containing current state and setpoint data + * @brief Publish a vortex_msgs Debug message containing current state and + * setpoint data * - * @param position_state A 3d vector containing the current body position - * @param orientation_state A quaternion containing the current orientation + * @param position_state A 3d vector containing the current body + * position + * @param orientation_state A quaternion containing the current + * orientation * @param velocity_state A 6d vector containing the current velocity * * @param position_setpoint A 3d vector containing the position setpoint - * @param orientation_setpoint A quaternion containing the orientation setpoint + * @param orientation_setpoint A quaternion containing the orientation + * setpoint */ - void publishDebugMsg(const Eigen::Vector3d& position_state, const Eigen::Quaterniond& orientation_state, - const Eigen::Vector6d& velocity_state, const Eigen::Vector3d& position_setpoint, - const Eigen::Quaterniond& orientation_setpoint); + void publishDebugMsg(const Eigen::Vector3d &position_state, + const Eigen::Quaterniond &orientation_state, + const Eigen::Vector6d &velocity_state, + const Eigen::Vector3d &position_setpoint, + const Eigen::Quaterniond &orientation_setpoint); /** * @brief Control mode for staying level * - * @param orientation_state A quaternion containing the current orientation + * @param orientation_state A quaternion containing the current + * orientation * @param velocity_state A 6d vector containing the current velocity * * @return A wrench vector for maintaining a level pose */ - Eigen::Vector6d stayLevel(const Eigen::Quaterniond& orientation_state, const Eigen::Vector6d& velocity_state); + Eigen::Vector6d stayLevel(const Eigen::Quaterniond &orientation_state, + const Eigen::Vector6d &velocity_state); /** * @brief Control mode for keeping constant depth * * - * @param position_state A 3d vector containing the current body position - * @param orientation_state A quaternion containing the current orientation + * @param position_state A 3d vector containing the current body + * position + * @param orientation_state A quaternion containing the current + * orientation * @param velocity_state A 6d vector containing the current velocity * * @param position_setpoint A 3d vector containing the position setpoint * * @return A wrench vector for maintaining constant depth */ - Eigen::Vector6d depthHold(const Eigen::Vector3d& position_state, const Eigen::Quaterniond& orientation_state, - const Eigen::Vector6d& velocity_state, const Eigen::Vector3d& position_setpoint); + Eigen::Vector6d depthHold(const Eigen::Vector3d &position_state, + const Eigen::Quaterniond &orientation_state, + const Eigen::Vector6d &velocity_state, + const Eigen::Vector3d &position_setpoint); /** * @brief Control mode for keeping a fixed heading * - * @param position_state A 3d vector containing the current body position - * @param orientation_state A quaternion containing the current orientation + * @param position_state A 3d vector containing the current body + * position + * @param orientation_state A quaternion containing the current + * orientation * @param velocity_state A 6d vector containing the current velocity * - * @param orientation_setpoint A quaternion containing the orientation setpoint + * @param orientation_setpoint A quaternion containing the orientation + * setpoint * * @return A wrench vector for maintaining constant heading */ - Eigen::Vector6d headingHold(const Eigen::Quaterniond& orientation_state, const Eigen::Vector6d& velocity_state, - const Eigen::Quaterniond& orientation_setpoint); + Eigen::Vector6d headingHold(const Eigen::Quaterniond &orientation_state, + const Eigen::Vector6d &velocity_state, + const Eigen::Quaterniond &orientation_setpoint); /** * @brief Control mode for keeping a fixed position * - * @param position_state A 3d vector containing the current body position - * @param orientation_state A quaternion containing the current orientation + * @param position_state A 3d vector containing the current body + * position + * @param orientation_state A quaternion containing the current + * orientation * @param velocity_state A 6d vector containing the current velocity * * @param position_setpoint A 3d vector containing the position setpoint - * @param orientation_setpoint A quaternion containing the orientation setpoint + * @param orientation_setpoint A quaternion containing the orientation + * setpoint * * @return A wrench vector for maintaining a fixed position */ - Eigen::Vector6d positionHold(const Eigen::Vector3d& position_state, const Eigen::Quaterniond& orientation_state, - const Eigen::Vector6d& velocity_state, const Eigen::Vector3d& position_setpoint, - const Eigen::Quaterniond& orientation_setpoint); + Eigen::Vector6d positionHold(const Eigen::Vector3d &position_state, + const Eigen::Quaterniond &orientation_state, + const Eigen::Vector6d &velocity_state, + const Eigen::Vector3d &position_setpoint, + const Eigen::Quaterniond &orientation_setpoint); /** * @brief Control mode for keeping both fixed position and fixed heading * - * @param position_state A 3d vector containing the current body position - * @param orientation_state A quaternion containing the current orientation + * @param position_state A 3d vector containing the current body + * position + * @param orientation_state A quaternion containing the current + * orientation * @param velocity_state A 6d vector containing the current velocity * * @param position_setpoint A 3d vector containing the position setpoint - * @param orientation_setpoint A quaternion containing the orientation setpoint + * @param orientation_setpoint A quaternion containing the orientation + * setpoint * * @return A wrench vector for maintaining both a fixed position and heading */ - Eigen::Vector6d positionHeadingHold(const Eigen::Vector3d& position_state, - const Eigen::Quaterniond& orientation_state, - const Eigen::Vector6d& velocity_state, const Eigen::Vector3d& position_setpoint, - const Eigen::Quaterniond& orientation_setpoint); + Eigen::Vector6d + positionHeadingHold(const Eigen::Vector3d &position_state, + const Eigen::Quaterniond &orientation_state, + const Eigen::Vector6d &velocity_state, + const Eigen::Vector3d &position_setpoint, + const Eigen::Quaterniond &orientation_setpoint); /** * @brief Control mode for keeping a fixed pose * - * @param position_state A 3d vector containing the current body position - * @param orientation_state A quaternion containing the current orientation + * @param position_state A 3d vector containing the current body + * position + * @param orientation_state A quaternion containing the current + * orientation * @param velocity_state A 6d vector containing the current velocity * * @param position_setpoint A 3d vector containing the position setpoint - * @param orientation_setpoint A quaternion containing the orientation setpoint + * @param orientation_setpoint A quaternion containing the orientation + * setpoint * * @return A feedback wrench for maintaining a fixed pose */ - Eigen::Vector6d poseHold(const Eigen::Vector3d& position_state, const Eigen::Quaterniond& orientation_state, - const Eigen::Vector6d& velocity_state, const Eigen::Vector3d& position_setpoint, - const Eigen::Quaterniond& orientation_setpoint); + Eigen::Vector6d poseHold(const Eigen::Vector3d &position_state, + const Eigen::Quaterniond &orientation_state, + const Eigen::Vector6d &velocity_state, + const Eigen::Vector3d &position_setpoint, + const Eigen::Quaterniond &orientation_setpoint); /** * @brief Control mode for keeping a fixed orientation * * @param orientation_state A quaternion containing the current orientation * @param velocity_state A 6d vector containing the current velocity - * @param orientation_setpoint A quaternion containing the orientation setpoint + * @param orientation_setpoint A quaternion containing the orientation + * setpoint * @return Eigen::Vector6d */ - Eigen::Vector6d orientationHold(const Eigen::Quaterniond& orientation_state, const Eigen::Vector6d& velocity_state, - const Eigen::Quaterniond& orientation_setpoint); + Eigen::Vector6d + orientationHold(const Eigen::Quaterniond &orientation_state, + const Eigen::Vector6d &velocity_state, + const Eigen::Quaterniond &orientation_setpoint); /** * @brief Control mode for keeping orientation and depth * - * @param position_state A 3d vector containing the current body position - * @param orientation_state A quaternion containing the current orientation + * @param position_state A 3d vector containing the current body + * position + * @param orientation_state A quaternion containing the current + * orientation * @param velocity_state A 6d vector containing the current velocity * * @param position_setpoint A 3d vector containing the position setpoint - * @param orientation_setpoint A quaternion containing the orientation setpoint + * @param orientation_setpoint A quaternion containing the orientation + * setpoint * * @return A feedback wrench for maintaining a fixed pose */ - Eigen::Vector6d orientationDepthHold(const Eigen::Vector3d& position_state, const Eigen::Quaterniond& orientation_state, - const Eigen::Vector6d& velocity_state, const Eigen::Vector3d& position_setpoint, - const Eigen::Quaterniond& orientation_setpoint); - + Eigen::Vector6d + orientationDepthHold(const Eigen::Vector3d &position_state, + const Eigen::Quaterniond &orientation_state, + const Eigen::Vector6d &velocity_state, + const Eigen::Vector3d &position_setpoint, + const Eigen::Quaterniond &orientation_setpoint); protected: - MoveBaseActionServer* mActionServer; /** Action server object */ + MoveBaseActionServer *mActionServer; /** Action server object */ move_base_msgs::MoveBaseFeedback feedback_; /** Current feedback value*/ @@ -333,4 +371,4 @@ class Controller geometry_msgs::PoseStamped mGoal; /** The current goal */ }; -#endif // VORTEX_CONTROLLER_CONTROLLER_ROS_H +#endif // VORTEX_CONTROLLER_CONTROLLER_ROS_H diff --git a/motion/dp_controller/include/dp_controller/eigen_helper.h b/motion/dp_controller/include/dp_controller/eigen_helper.h index c7fec1d3e..3451723b5 100644 --- a/motion/dp_controller/include/dp_controller/eigen_helper.h +++ b/motion/dp_controller/include/dp_controller/eigen_helper.h @@ -2,14 +2,13 @@ * @file * @brief An extension of the eigen library containing useful * functionality for matrix computation -*/ - + */ #ifndef VORTEX_EIGEN_HELPER_H #define VORTEX_EIGEN_HELPER_H -#include "vortex_msgs/ThrusterForces.h" #include "ros/ros.h" +#include "vortex_msgs/ThrusterForces.h" #include #include @@ -17,45 +16,41 @@ /** * @brief check if a matrix is valid (no nan or inf entries) - * + * * @param X The matrix to be checked - * + * * @return true if X has any nan or inf elements, false otherwise -*/ -template -inline bool isFucked(const Eigen::MatrixBase& X) -{ + */ +template +inline bool isFucked(const Eigen::MatrixBase &X) { bool has_nan = !(X.array() == X.array()).all(); bool has_inf = !((X - X).array() == (X - X).array()).all(); return has_nan || has_inf; } - /** * @brief read a matrix from the ROS parameter server - * + * * @param nh ROS NodeHandle * @param name Parameter name of the matrix in the parameter server - * @param X The matrix that will be filled with the values read from the parameter server - * + * @param X The matrix that will be filled with the values read from the + * parameter server + * * @return false if unsuccessful, true if successful -*/ -inline bool getMatrixParam(ros::NodeHandle nh, std::string name, Eigen::MatrixXd *X) -{ + */ +inline bool getMatrixParam(ros::NodeHandle nh, std::string name, + Eigen::MatrixXd *X) { XmlRpc::XmlRpcValue matrix; nh.getParam(name, matrix); - try - { + try { const int rows = matrix.size(); const int cols = matrix[0].size(); X->setZero(rows, cols); for (int i = 0; i < rows; ++i) for (int j = 0; j < cols; ++j) (*X)(i, j) = matrix[i][j]; - } - catch(...) - { + } catch (...) { return false; } return true; @@ -63,65 +58,56 @@ inline bool getMatrixParam(ros::NodeHandle nh, std::string name, Eigen::MatrixXd /** * @brief print Eigen matrix - * + * * @param name Name of matrix * @param X Matrix -*/ -inline void printEigen(std::string name, const Eigen::MatrixXd &X) -{ + */ +inline void printEigen(std::string name, const Eigen::MatrixXd &X) { std::stringstream ss; ss << std::endl << name << " = " << std::endl << X; ROS_INFO_STREAM(ss.str()); } - /** * @brief calculate pseudoinverse matrix - * + * * @param X The matrix to be inverted * @param X_pinv The inverted version of the @p X matrix - * + * * @return false if the calculation fails, true otherwise -*/ -inline bool pseudoinverse(const Eigen::MatrixXd &X, Eigen::MatrixXd *X_pinv) -{ + */ +inline bool pseudoinverse(const Eigen::MatrixXd &X, Eigen::MatrixXd *X_pinv) { Eigen::MatrixXd copy = X.transpose() * (X * X.transpose()).inverse(); - if (isFucked(copy)) - { + if (isFucked(copy)) { return false; } *X_pinv = copy; return true; } - /** * @brief calculate a 3x3 skew-symmetric matrix - * + * * @param v The vector from which the matrix is to be calculated - * + * * @return the 3x3 skew-symmetric matrix of @p v -*/ -inline Eigen::Matrix3d skew(const Eigen::Vector3d &v) -{ + */ +inline Eigen::Matrix3d skew(const Eigen::Vector3d &v) { Eigen::Matrix3d S; - S << 0 , -v(2), v(1), - v(2), 0 , -v(0), - -v(1), v(0), 0; + S << 0, -v(2), v(1), v(2), 0, -v(0), -v(1), v(0), 0; return S; } - /** * @brief Convert a vector to a ThrusterForces vortex message - * + * * @param u The Eigen::vector to be converted - * @param msg The vortex_msgs output message - * -*/ -inline void arrayEigenToMsg(const Eigen::VectorXd &u, vortex_msgs::ThrusterForces *msg) -{ + * @param msg The vortex_msgs output message + * + */ +inline void arrayEigenToMsg(const Eigen::VectorXd &u, + vortex_msgs::ThrusterForces *msg) { int r = u.size(); std::vector u_vec(r); for (int i = 0; i < r; ++i) @@ -129,30 +115,24 @@ inline void arrayEigenToMsg(const Eigen::VectorXd &u, vortex_msgs::ThrusterForce msg->thrust = u_vec; } - /** * @brief saturate a vector to a given range - * + * * @param v The vector to be saturated * @param min Lower saturation limit * @param max Upper saturation limit - * + * * @return true if all elements are in the [ @p min @p max ] range already * false otherwise - * -*/ -inline bool saturateVector(Eigen::VectorXd *v, double min, double max) -{ + * + */ +inline bool saturateVector(Eigen::VectorXd *v, double min, double max) { bool vector_in_range = true; - for (int i = 0; i < v->size(); ++i) - { - if ((*v)(i) > max) - { + for (int i = 0; i < v->size(); ++i) { + if ((*v)(i) > max) { (*v)(i) = max; vector_in_range = false; - } - else if ((*v)(i) < min) - { + } else if ((*v)(i) < min) { (*v)(i) = min; vector_in_range = false; } @@ -160,4 +140,4 @@ inline bool saturateVector(Eigen::VectorXd *v, double min, double max) return vector_in_range; } -#endif // VORTEX_EIGEN_HELPER_H +#endif // VORTEX_EIGEN_HELPER_H diff --git a/motion/dp_controller/include/dp_controller/eigen_typedefs.h b/motion/dp_controller/include/dp_controller/eigen_typedefs.h index 2b3266386..72dbc857f 100644 --- a/motion/dp_controller/include/dp_controller/eigen_typedefs.h +++ b/motion/dp_controller/include/dp_controller/eigen_typedefs.h @@ -3,10 +3,9 @@ #include -namespace Eigen -{ +namespace Eigen { typedef Eigen::Matrix Matrix6d; typedef Eigen::Matrix Vector6d; -} +} // namespace Eigen -#endif // VORTEX_EIGEN_TYPEDEFS_H +#endif // VORTEX_EIGEN_TYPEDEFS_H diff --git a/motion/dp_controller/include/dp_controller/quaternion_pd_controller.h b/motion/dp_controller/include/dp_controller/quaternion_pd_controller.h index 9ab744cd7..7c0987f8e 100644 --- a/motion/dp_controller/include/dp_controller/quaternion_pd_controller.h +++ b/motion/dp_controller/include/dp_controller/quaternion_pd_controller.h @@ -20,11 +20,11 @@ #ifndef VORTEX_CONTROLLER_QUATERNION_PD_CONTROLLER_H #define VORTEX_CONTROLLER_QUATERNION_PD_CONTROLLER_H -#include -#include #include #include +#include #include +#include #include "eigen_typedefs.h" using namespace Eigen; @@ -32,25 +32,24 @@ using namespace Eigen; /** * @brief A complete class foinitr the nonlinear PID controller */ -class QuaternionPdController -{ +class QuaternionPdController { public: - /** * @brief Controller class initiator * * @param a Diagonal value for derivative gain matrix * @param b Diagonal value for position gain matrix * @param c Orientation gain - * @param i Scalar for orientation integral gain and diagonal value for integral gain matrix + * @param i Scalar for orientation integral gain and diagonal value for + * integral gain matrix * @param W Weight of drone * @param B Buoyancy of drone * @param r_G Center of gravity, expressed in body frame * @param r_B Center of buoyancy, expressed in body frame * */ - void init(double a, double b, double c, double i, double W, double B, const Eigen::Vector3d& r_G, - const Eigen::Vector3d& r_B); + void init(double a, double b, double c, double i, double W, double B, + const Eigen::Vector3d &r_G, const Eigen::Vector3d &r_B); /** * @brief set the gains of the controller @@ -58,7 +57,8 @@ class QuaternionPdController * @param a Diagonal value for derivative gain matrix * @param b Diagonal value for position gain matrix * @param c Orientation gain - * @param i Scalar for orientation integral gain and diagonal value for integral gain matrix + * @param i Scalar for orientation integral gain and diagonal value for + * integral gain matrix * */ void setGains(double a, double b, double c, double i); @@ -71,7 +71,7 @@ class QuaternionPdController * * @return the restoring forces vector */ - Eigen::Vector6d getRestoring(const Eigen::Quaterniond& q); + Eigen::Vector6d getRestoring(const Eigen::Quaterniond &q); /** * @brief a getter for the feedback vector @@ -86,21 +86,27 @@ class QuaternionPdController * * @return Control vector without restoring forces */ - Eigen::Vector6d getFeedback(const Eigen::Vector3d& x, const Eigen::Quaterniond& q, const Eigen::Vector6d& nu, - const Eigen::Vector3d& x_d, const Eigen::Quaterniond& q_d); + Eigen::Vector6d getFeedback(const Eigen::Vector3d &x, + const Eigen::Quaterniond &q, + const Eigen::Vector6d &nu, + const Eigen::Vector3d &x_d, + const Eigen::Quaterniond &q_d); /** - * @brief Utilize a calculated reference model to find next desired body position + * @brief Utilize a calculated reference model to find next desired body + * position * * @param x The current body position, as a 3d vector * @param x_ref The body position reference, as a 3d vector * * @return Next desired body position */ - Eigen::Vector3d referenceModel(const Eigen::Vector3d& x, const Eigen::Vector3d& x_ref); + Eigen::Vector3d referenceModel(const Eigen::Vector3d &x, + const Eigen::Vector3d &x_ref); /** - * @brief Check if desired position is within a defined radius of the current position + * @brief Check if desired position is within a defined radius of the current + * position * * @param x The current body position, as a 3d vector * @param x_d The desired body position, as a 3d vector @@ -108,13 +114,19 @@ class QuaternionPdController * * @return true if inside circle of acceptance, false if not. */ - bool circleOfAcceptance(const Eigen::Vector3d& x, const Eigen::Vector3d& x_d, float R); - - Eigen::Vector6d integral = Eigen::Vector6d::Zero(); /** Integral error vector */ - Eigen::Vector3d x_d_prev = Eigen::Vector3d::Zero(); /** Previous desired body position */ - Eigen::Vector3d x_d_prev_prev = Eigen::Vector3d::Zero(); /** Previous previous desired body position */ - Eigen::Vector3d x_ref_prev = Eigen::Vector3d::Zero(); /** Previous reference body position */ - Eigen::Vector3d x_ref_prev_prev = Eigen::Vector3d::Zero(); /** Previous previous reference body position */ + bool circleOfAcceptance(const Eigen::Vector3d &x, const Eigen::Vector3d &x_d, + float R); + + Eigen::Vector6d integral = + Eigen::Vector6d::Zero(); /** Integral error vector */ + Eigen::Vector3d x_d_prev = + Eigen::Vector3d::Zero(); /** Previous desired body position */ + Eigen::Vector3d x_d_prev_prev = + Eigen::Vector3d::Zero(); /** Previous previous desired body position */ + Eigen::Vector3d x_ref_prev = + Eigen::Vector3d::Zero(); /** Previous reference body position */ + Eigen::Vector3d x_ref_prev_prev = + Eigen::Vector3d::Zero(); /** Previous previous reference body position */ EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -150,7 +162,7 @@ class QuaternionPdController * @param att_lim The maximum allowed attitude gain * */ - void integralWindUp(Eigen::Vector6d& vec, double pose_lim, double att_lim); + void integralWindUp(Eigen::Vector6d &vec, double pose_lim, double att_lim); /** * @brief Calculate error vector. @@ -167,8 +179,10 @@ class QuaternionPdController * * @return a 6x1 vector consisting of the error in position and attitudes. */ - Eigen::Vector6d errorVector(const Eigen::Vector3d& x, const Eigen::Vector3d& x_d, const Eigen::Quaterniond& q, - const Eigen::Quaterniond& q_d); + Eigen::Vector6d errorVector(const Eigen::Vector3d &x, + const Eigen::Vector3d &x_d, + const Eigen::Quaterniond &q, + const Eigen::Quaterniond &q_d); /** * @brief calculate the restoring force vector @@ -201,4 +215,4 @@ class QuaternionPdController double m_B; /** [N] Buoyancy of drone */ }; -#endif // VORTEX_CONTROLLER_QUATERNION_PD_CONTROLLER_H +#endif // VORTEX_CONTROLLER_QUATERNION_PD_CONTROLLER_H diff --git a/motion/dp_controller/include/dp_controller/setpoints.h b/motion/dp_controller/include/dp_controller/setpoints.h index 71948a7ae..56bbe23ca 100644 --- a/motion/dp_controller/include/dp_controller/setpoints.h +++ b/motion/dp_controller/include/dp_controller/setpoints.h @@ -6,114 +6,112 @@ /** * @file * @brief Class declaration for the Setpoint class -*/ - + */ #ifndef VORTEX_CONTROLLER_SETPOINTS_H #define VORTEX_CONTROLLER_SETPOINTS_H -#include #include "eigen_typedefs.h" - +#include /** - * @brief a class containing the current setpoint data for wrench, position and orientation -*/ -class Setpoints -{ + * @brief a class containing the current setpoint data for wrench, position and + * orientation + */ +class Setpoints { public: /** * @brief Setpoints class constructor - * - */ + * + */ Setpoints(); - /** * @brief Get wrench setpoint - * + * * @param wrench A pointer whos values will be set to zeros - * + * * @return false if current wrench setpoint is invalid, true otherwise - */ + */ bool getZero(Eigen::Vector6d *wrench); - /** * @brief Get position and orientation setpoints - * - * @param position A pointer whos value will be set to the current position setpoint - * @param orientation A pointer whos value will be set to the current orientation setpoint - * + * + * @param position A pointer whos value will be set to the current + * position setpoint + * @param orientation A pointer whos value will be set to the current + * orientation setpoint + * * @return false if current pose is invalid, true otherwise - */ - bool get(Eigen::Vector3d *position, - Eigen::Quaterniond *orientation); - + */ + bool get(Eigen::Vector3d *position, Eigen::Quaterniond *orientation); /** * @brief Get position setpoint - * - * @param position A pointer whos value will be set to the current position setpoint - * + * + * @param position A pointer whos value will be set to the current + * position setpoint + * * @return false if current pose is invalid, true otherwise - */ + */ bool get(Eigen::Vector3d *position); - /** * @brief Get orientation setpoint - * - * @param orientation A pointer whos value will be set to the current orientation setpoint - * + * + * @param orientation A pointer whos value will be set to the current + * orientation setpoint + * * @return false if current pose is invalid, true otherwise - */ + */ bool get(Eigen::Quaterniond *orientation); - /** * @brief Get orientation setpoint in euler angles - * - * @param orientation A pointer whos value will be set to the current orientation setpoint - * + * + * @param orientation A pointer whos value will be set to the current + * orientation setpoint + * * @return false if current pose is invalid, true otherwise - */ + */ bool getEuler(Eigen::Vector3d *orientation); - /** * @brief Set position and orientation setpoints - * + * * @param position The new position setpoint, expressed as a 3d vector - * @param orientation The new orientation setpoint, expressed as a quaternion - */ - void set(const Eigen::Vector3d &position, + * @param orientation The new orientation setpoint, expressed as a + * quaternion + */ + void set(const Eigen::Vector3d &position, const Eigen::Quaterniond &orientation); - /** * @brief Set position setpoint - * + * * @param position The new position setpoint, expressed as a 3d vector - */ - void set(const Eigen::Vector3d &position); - + */ + void set(const Eigen::Vector3d &position); /** * @brief Set orientation setpoint - * - * @param orientation The new orientation setpoint, expressed as a quaternion * - */ + * @param orientation The new orientation setpoint, expressed as a + * quaternion + * + */ void set(const Eigen::Quaterniond &orientation); EIGEN_MAKE_ALIGNED_OPERATOR_NEW private: - Eigen::Vector3d m_position; /** 6d vector containing position setpoint values */ - Eigen::Quaterniond m_orientation; /** Quaternion containing orientation setpoints */ + Eigen::Vector3d + m_position; /** 6d vector containing position setpoint values */ + Eigen::Quaterniond + m_orientation; /** Quaternion containing orientation setpoints */ - bool m_wrench_is_valid; /** Determines if current wrench values are valid */ - bool m_pose_is_valid; /** Determines if current pose is valid */ + bool m_wrench_is_valid; /** Determines if current wrench values are valid */ + bool m_pose_is_valid; /** Determines if current pose is valid */ }; -#endif // VORTEX_CONTROLLER_SETPOINTS_H +#endif // VORTEX_CONTROLLER_SETPOINTS_H diff --git a/motion/dp_controller/include/dp_controller/state.h b/motion/dp_controller/include/dp_controller/state.h index 9897d2af9..544afc34b 100644 --- a/motion/dp_controller/include/dp_controller/state.h +++ b/motion/dp_controller/include/dp_controller/state.h @@ -6,112 +6,111 @@ /** * @file * @brief Class declaration for the Setpoint class -*/ - + */ #ifndef VORTEX_CONTROLLER_STATE_H #define VORTEX_CONTROLLER_STATE_H -#include #include "eigen_typedefs.h" +#include using namespace Eigen; /** - * @brief a class containing the current state data for position, orientation and velocity -*/ -class State -{ + * @brief a class containing the current state data for position, orientation + * and velocity + */ +class State { public: - /** * @brief State class constructor - * + * * States for position and velocity are all set to zero, * while orientation is initialized as the 6x6 identity matrix. - */ + */ State(); - /** * @brief Get position and orientation states - * - * @param position A pointer whos value will be set to the current position state - * @param orientation A pointer whos value will be set to the current orientation state - * + * + * @param position A pointer whos value will be set to the current + * position state + * @param orientation A pointer whos value will be set to the current + * orientation state + * * @return false if states are not initialized, true otherwise - */ - bool get(Eigen::Vector3d *position, - Eigen::Quaterniond *orientation); - + */ + bool get(Eigen::Vector3d *position, Eigen::Quaterniond *orientation); /** * @brief Get position, orientation and velocity states - * - * @param position A pointer whos value will be set to the current position state - * @param orientation A pointer whos value will be set to the current orientation state - * @param velocity A pointer whos value will be set to the current velocity state - * + * + * @param position A pointer whos value will be set to the current + * position state + * @param orientation A pointer whos value will be set to the current + * orientation state + * @param velocity A pointer whos value will be set to the current + * velocity state + * * @return false if states are not initialized, true otherwise - */ - bool get(Eigen::Vector3d *position, - Eigen::Quaterniond *orientation, - Eigen::Vector6d *velocity); - + */ + bool get(Eigen::Vector3d *position, Eigen::Quaterniond *orientation, + Eigen::Vector6d *velocity); /** * @brief Get position state - * - * @param position A pointer whos value will be set to the current position state - * + * + * @param position A pointer whos value will be set to the current + * position state + * * @return false if states are not initialized, true otherwise - */ - bool get(Eigen::Vector3d *position); - + */ + bool get(Eigen::Vector3d *position); /** * @brief Get orientation state - * - * @param orientation A pointer whos value will be set to the current orientation state - * + * + * @param orientation A pointer whos value will be set to the current + * orientation state + * * @return false if states are not initialized, true otherwise - */ - bool get(Eigen::Quaterniond *orientation); - + */ + bool get(Eigen::Quaterniond *orientation); /** * @brief Get orientation state in euler angles - * - * @param orientation A pointer whos value will be set to the euler orientation - * + * + * @param orientation A pointer whos value will be set to the euler + * orientation + * * @return false if states are not initialized, true otherwise - */ + */ bool getEuler(Eigen::Vector3d *orientation); - /** * @brief Set position, orientation and velocity states - * - * @param position A 3d vector containing the position to be set + * + * @param position A 3d vector containing the position to be set * @param orientation A quaternion containing the orientation to be set * @param velocity A 6d vector containing the velocity to be set - * - * @warning All state values are considered invalid until this function is called. - */ - void set(const Eigen::Vector3d &position, + * + * @warning All state values are considered invalid until this function is + * called. + */ + void set(const Eigen::Vector3d &position, const Eigen::Quaterniond &orientation, - const Eigen::Vector6d &velocity); - + const Eigen::Vector6d &velocity); EIGEN_MAKE_ALIGNED_OPERATOR_NEW private: + Eigen::Vector3d m_position; /** A 3d vector containing the position state*/ + Eigen::Quaterniond + m_orientation; /** A quaternion containing the orientation state*/ + Eigen::Vector6d m_velocity; /** A 6d vector containing the velocity state*/ - Eigen::Vector3d m_position; /** A 3d vector containing the position state*/ - Eigen::Quaterniond m_orientation; /** A quaternion containing the orientation state*/ - Eigen::Vector6d m_velocity; /** A 6d vector containing the velocity state*/ - - bool m_is_initialized; /** A bool to indicate whether or not the states are initialized. False before set() is called*/ + bool m_is_initialized; /** A bool to indicate whether or not the states are + initialized. False before set() is called*/ }; -#endif // VORTEX_CONTROLLER_STATE_H +#endif // VORTEX_CONTROLLER_STATE_H diff --git a/motion/dp_controller/src/controller_node.cpp b/motion/dp_controller/src/controller_node.cpp index 7be9eb251..6b3470b41 100644 --- a/motion/dp_controller/src/controller_node.cpp +++ b/motion/dp_controller/src/controller_node.cpp @@ -1,12 +1,11 @@ -/* +/* Written by Kristoffer Rakstad Solberg, Student Copyright (c) 2019 Manta AUV, Vortex NTNU. All rights reserved. */ #include "dp_controller/controller_ros.h" -int main(int argc, char **argv) -{ +int main(int argc, char **argv) { ros::init(argc, argv, "dp_controller"); ros::NodeHandle nh; Controller controller(nh); diff --git a/motion/dp_controller/src/controller_ros.cpp b/motion/dp_controller/src/controller_ros.cpp index 16797f7b4..3df1340b4 100644 --- a/motion/dp_controller/src/controller_ros.cpp +++ b/motion/dp_controller/src/controller_ros.cpp @@ -4,8 +4,7 @@ #include "dp_controller/controller_ros.h" -Controller::Controller(ros::NodeHandle nh) : m_nh(nh) -{ +Controller::Controller(ros::NodeHandle nh) : m_nh(nh) { // Load rosparams std::string odometry_topic; std::string thrust_topic; @@ -19,50 +18,47 @@ Controller::Controller(ros::NodeHandle nh) : m_nh(nh) if (!nh.getParam("/thrust/thrust_topic", thrust_topic)) thrust_topic = "/thrust/desired_forces"; - if (!m_nh.getParam("/controllers/dp/velocity_gain", a)) - { + if (!m_nh.getParam("/controllers/dp/velocity_gain", a)) { ROS_FATAL("Failed to read parameter velocity_gain."); ros::shutdown(); } - if (!m_nh.getParam("/controllers/dp/position_gain", b)) - { + if (!m_nh.getParam("/controllers/dp/position_gain", b)) { ROS_FATAL("Failed to read parameter position_gain."); ros::shutdown(); } - if (!m_nh.getParam("/controllers/dp/attitude_gain", c)) - { + if (!m_nh.getParam("/controllers/dp/attitude_gain", c)) { ROS_FATAL("Failed to read parameter attitude_gain."); ros::shutdown(); } - if (!m_nh.getParam("/controllers/dp/integral_gain", i)) - { + if (!m_nh.getParam("/controllers/dp/integral_gain", i)) { ROS_FATAL("Failed to read parameter integral_gain."); ros::shutdown(); } - if (!m_nh.getParam("/physical/weight", W)) - { + if (!m_nh.getParam("/physical/weight", W)) { ROS_FATAL("Failed to read parameter physical/weight. Shutting down node.."); ros::shutdown(); } - if (!m_nh.getParam("/physical/buoyancy", B)) - { - ROS_FATAL("Failed to read parameter physical/buoyancy. Shutting down node.."); + if (!m_nh.getParam("/physical/buoyancy", B)) { + ROS_FATAL( + "Failed to read parameter physical/buoyancy. Shutting down node.."); ros::shutdown(); } - if (!m_nh.getParam("/physical/center_of_mass", r_G_vec)) - { - ROS_FATAL("Failed to read parameter physical/center_of_mass. Shutting down node.."); + if (!m_nh.getParam("/physical/center_of_mass", r_G_vec)) { + ROS_FATAL("Failed to read parameter physical/center_of_mass. Shutting down " + "node.."); ros::shutdown(); } - if (!m_nh.getParam("/physical/center_of_buoyancy", r_B_vec)) - { - ROS_FATAL("Failed to read parameter physical/center_of_buoyancy. Shutting down node.."); + if (!m_nh.getParam("/physical/center_of_buoyancy", r_B_vec)) { + ROS_FATAL("Failed to read parameter physical/center_of_buoyancy. Shutting " + "down node.."); ros::shutdown(); } // calculate vectors to CG and CB in meters - Eigen::Vector3d r_G = Eigen::Vector3d(r_G_vec[0], r_G_vec[1], r_G_vec[2]) / 1000; // convert from mm to m - Eigen::Vector3d r_B = Eigen::Vector3d(r_B_vec[0], r_B_vec[1], r_B_vec[2]) / 1000; + Eigen::Vector3d r_G = Eigen::Vector3d(r_G_vec[0], r_G_vec[1], r_G_vec[2]) / + 1000; // convert from mm to m + Eigen::Vector3d r_B = + Eigen::Vector3d(r_B_vec[0], r_B_vec[1], r_B_vec[2]) / 1000; // Initiate variables prev_control_mode = ControlModes::OPEN_LOOP; @@ -72,7 +68,8 @@ Controller::Controller(ros::NodeHandle nh) : m_nh(nh) velocity = Eigen::VectorXd::Zero(6); // Set up a dynamic reconfigure server - dynamic_reconfigure::Server::CallbackType dr_cb; + dynamic_reconfigure::Server< + dp_controller::VortexControllerConfig>::CallbackType dr_cb; dr_cb = boost::bind(&Controller::configCallback, this, _1, _2); m_dr_srv.setCallback(dr_cb); @@ -85,19 +82,19 @@ Controller::Controller(ros::NodeHandle nh) : m_nh(nh) m_debug_pub = m_nh.advertise("debug/controlstates", 10); // Subscribers - m_state_sub = m_nh.subscribe(odometry_topic, 1, &Controller::stateCallback, this); - m_guidance_sub = m_nh.subscribe("dp_data", 1, &Controller::guidanceCallback, this); + m_state_sub = + m_nh.subscribe(odometry_topic, 1, &Controller::stateCallback, this); + m_guidance_sub = + m_nh.subscribe("dp_data", 1, &Controller::guidanceCallback, this); ROS_INFO("DP controller initialized"); } -ControlMode Controller::getControlMode(int mode) -{ +ControlMode Controller::getControlMode(int mode) { return static_cast(mode); } -void Controller::stateCallback(const nav_msgs::Odometry& msg) -{ +void Controller::stateCallback(const nav_msgs::Odometry &msg) { // Convert to eigen for computation Eigen::Vector3d position_tmp; Eigen::Quaterniond orientation_tmp; @@ -106,14 +103,12 @@ void Controller::stateCallback(const nav_msgs::Odometry& msg) tf::quaternionMsgToEigen(msg.pose.pose.orientation, orientation_tmp); tf::twistMsgToEigen(msg.twist.twist, velocity_tmp); - bool orientation_invalid = (abs(orientation_tmp.norm() - 1) > c_max_quat_norm_deviation); - if (isFucked(position_tmp) || isFucked(velocity_tmp) || orientation_invalid) - { + bool orientation_invalid = + (abs(orientation_tmp.norm() - 1) > c_max_quat_norm_deviation); + if (isFucked(position_tmp) || isFucked(velocity_tmp) || orientation_invalid) { ROS_WARN_THROTTLE(1, "Invalid state estimate received, ignoring..."); return; - } - else - { + } else { // Update local states position, orientation and velocity position = position_tmp; orientation = orientation_tmp; @@ -122,18 +117,19 @@ void Controller::stateCallback(const nav_msgs::Odometry& msg) } } -void Controller::guidanceCallback(const vortex_msgs::DpSetpoint& msg) -{ +void Controller::guidanceCallback(const vortex_msgs::DpSetpoint &msg) { // gets the newest state as Eigen - position_setpoint = Eigen::Vector3d{msg.setpoint.position.x, msg.setpoint.position.y, msg.setpoint.position.z}; - orientation_setpoint = Eigen::Quaterniond{msg.setpoint.orientation.w, msg.setpoint.orientation.x, - msg.setpoint.orientation.y, msg.setpoint.orientation.z}; + position_setpoint = + Eigen::Vector3d{msg.setpoint.position.x, msg.setpoint.position.y, + msg.setpoint.position.z}; + orientation_setpoint = Eigen::Quaterniond{ + msg.setpoint.orientation.w, msg.setpoint.orientation.x, + msg.setpoint.orientation.y, msg.setpoint.orientation.z}; // check control mode ControlMode control_mode = getControlMode(msg.control_mode); - if (control_mode != prev_control_mode) - { + if (control_mode != prev_control_mode) { prev_control_mode = control_mode; // If control mode is OPENLOOP, publish zero control force @@ -154,25 +150,29 @@ void Controller::guidanceCallback(const vortex_msgs::DpSetpoint& msg) // Integral action reset m_controller.integral = Eigen::Vector6d::Zero(); - ROS_INFO_STREAM("Changing mode to " << controlModeString(control_mode) << "."); + ROS_INFO_STREAM("Changing mode to " << controlModeString(control_mode) + << "."); } } -void Controller::configCallback(const dp_controller::VortexControllerConfig& config, uint32_t level) -{ +void Controller::configCallback( + const dp_controller::VortexControllerConfig &config, uint32_t level) { ROS_INFO("DP controller reconfigure:"); ROS_INFO("\t velocity_gain: %2.4f", config.velocity_gain); ROS_INFO("\t position_gain: %2.4f", config.position_gain); ROS_INFO("\t attitude_gain: %2.4f", config.attitude_gain); ROS_INFO("\t integral_gain: %2.4f", config.integral_gain); - m_controller.setGains(config.velocity_gain, config.position_gain, config.attitude_gain, config.integral_gain); + m_controller.setGains(config.velocity_gain, config.position_gain, + config.attitude_gain, config.integral_gain); } -void Controller::publishDebugMsg(const Eigen::Vector3d& position_state, const Eigen::Quaterniond& orientation_state, - const Eigen::Vector6d& velocity_state, const Eigen::Vector3d& position_setpoint, - const Eigen::Quaterniond& orientation_setpoint) -{ +void Controller::publishDebugMsg( + const Eigen::Vector3d &position_state, + const Eigen::Quaterniond &orientation_state, + const Eigen::Vector6d &velocity_state, + const Eigen::Vector3d &position_setpoint, + const Eigen::Quaterniond &orientation_setpoint) { vortex_msgs::Debug dbg_msg; // Estimated position @@ -196,8 +196,10 @@ void Controller::publishDebugMsg(const Eigen::Vector3d& position_state, const Ei dbg_msg.setpoint_position.z = position_setpoint[2]; // Debub state euler orientation - Eigen::Vector3d dbg_state_orientation = orientation_state.toRotationMatrix().eulerAngles(2, 1, 0); - Eigen::Vector3d dbg_setpoint_orientation = orientation_setpoint.toRotationMatrix().eulerAngles(2, 1, 0); + Eigen::Vector3d dbg_state_orientation = + orientation_state.toRotationMatrix().eulerAngles(2, 1, 0); + Eigen::Vector3d dbg_setpoint_orientation = + orientation_setpoint.toRotationMatrix().eulerAngles(2, 1, 0); // Debug state orientation dbg_msg.state_yaw = dbg_state_orientation[0]; @@ -213,21 +215,25 @@ void Controller::publishDebugMsg(const Eigen::Vector3d& position_state, const Ei m_debug_pub.publish(dbg_msg); } -Eigen::Vector6d Controller::depthHold(const Eigen::Vector3d& position_state, - const Eigen::Quaterniond& orientation_state, - const Eigen::Vector6d& velocity_state, const Eigen::Vector3d& position_setpoint) -{ - Eigen::Vector6d tau = m_controller.getFeedback(Eigen::Vector3d{0, 0, position_state[2]}, orientation_state, velocity_state, - Eigen::Vector3d{0, 0, position_setpoint[2]}, orientation_state); +Eigen::Vector6d +Controller::depthHold(const Eigen::Vector3d &position_state, + const Eigen::Quaterniond &orientation_state, + const Eigen::Vector6d &velocity_state, + const Eigen::Vector3d &position_setpoint) { + Eigen::Vector6d tau = m_controller.getFeedback( + Eigen::Vector3d{0, 0, position_state[2]}, orientation_state, + velocity_state, Eigen::Vector3d{0, 0, position_setpoint[2]}, + orientation_state); return tau; } -Eigen::Vector6d Controller::headingHold(const Eigen::Quaterniond& orientation_state, - const Eigen::Vector6d& velocity_state, - const Eigen::Quaterniond& orientation_setpoint) -{ - Eigen::Vector6d tau = m_controller.getFeedback(Eigen::Vector3d::Zero(), orientation_state, velocity_state, - Eigen::Vector3d::Zero(), orientation_setpoint); +Eigen::Vector6d +Controller::headingHold(const Eigen::Quaterniond &orientation_state, + const Eigen::Vector6d &velocity_state, + const Eigen::Quaterniond &orientation_setpoint) { + Eigen::Vector6d tau = m_controller.getFeedback( + Eigen::Vector3d::Zero(), orientation_state, velocity_state, + Eigen::Vector3d::Zero(), orientation_setpoint); // Allow only yaw feedback command tau(SURGE) = 0; @@ -239,14 +245,15 @@ Eigen::Vector6d Controller::headingHold(const Eigen::Quaterniond& orientation_st return tau; } -Eigen::Vector6d Controller::positionHold(const Eigen::Vector3d& position_state, - const Eigen::Quaterniond& orientation_state, - const Eigen::Vector6d& velocity_state, - const Eigen::Vector3d& position_setpoint, - const Eigen::Quaterniond& orientation_setpoint) -{ - Eigen::Vector6d tau = m_controller.getFeedback(position_state, orientation_state, velocity_state, position_setpoint, - orientation_setpoint); +Eigen::Vector6d +Controller::positionHold(const Eigen::Vector3d &position_state, + const Eigen::Quaterniond &orientation_state, + const Eigen::Vector6d &velocity_state, + const Eigen::Vector3d &position_setpoint, + const Eigen::Quaterniond &orientation_setpoint) { + Eigen::Vector6d tau = m_controller.getFeedback( + position_state, orientation_state, velocity_state, position_setpoint, + orientation_setpoint); // Allow only surge,sway,heave feedback command tau(ROLL) = 0; @@ -256,14 +263,15 @@ Eigen::Vector6d Controller::positionHold(const Eigen::Vector3d& position_state, return tau; } -Eigen::Vector6d Controller::positionHeadingHold(const Eigen::Vector3d& position_state, - const Eigen::Quaterniond& orientation_state, - const Eigen::Vector6d& velocity_state, - const Eigen::Vector3d& position_setpoint, - const Eigen::Quaterniond& orientation_setpoint) -{ - Eigen::Vector6d tau = m_controller.getFeedback(position_state, orientation_state, velocity_state, position_setpoint, - orientation_setpoint); +Eigen::Vector6d Controller::positionHeadingHold( + const Eigen::Vector3d &position_state, + const Eigen::Quaterniond &orientation_state, + const Eigen::Vector6d &velocity_state, + const Eigen::Vector3d &position_setpoint, + const Eigen::Quaterniond &orientation_setpoint) { + Eigen::Vector6d tau = m_controller.getFeedback( + position_state, orientation_state, velocity_state, position_setpoint, + orientation_setpoint); tau(ROLL) = 0; tau(PITCH) = 0; @@ -271,101 +279,118 @@ Eigen::Vector6d Controller::positionHeadingHold(const Eigen::Vector3d& position_ return tau; } -Eigen::Vector6d Controller::poseHold(const Eigen::Vector3d& position_state, const Eigen::Quaterniond& orientation_state, - const Eigen::Vector6d& velocity_state, const Eigen::Vector3d& position_setpoint, - const Eigen::Quaterniond& orientation_setpoint) -{ - Eigen::Vector6d tau = m_controller.getFeedback(position_state, orientation_state, velocity_state, position_setpoint, - orientation_setpoint); +Eigen::Vector6d +Controller::poseHold(const Eigen::Vector3d &position_state, + const Eigen::Quaterniond &orientation_state, + const Eigen::Vector6d &velocity_state, + const Eigen::Vector3d &position_setpoint, + const Eigen::Quaterniond &orientation_setpoint) { + Eigen::Vector6d tau = m_controller.getFeedback( + position_state, orientation_state, velocity_state, position_setpoint, + orientation_setpoint); return tau; } -Eigen::Vector6d Controller::orientationHold(const Eigen::Quaterniond& orientation_state, - const Eigen::Vector6d& velocity_state, - const Eigen::Quaterniond& orientation_setpoint) -{ - Eigen::Vector6d tau = m_controller.getFeedback(Eigen::Vector3d::Zero(), orientation_state, velocity_state, - Eigen::Vector3d::Zero(), orientation_setpoint); +Eigen::Vector6d +Controller::orientationHold(const Eigen::Quaterniond &orientation_state, + const Eigen::Vector6d &velocity_state, + const Eigen::Quaterniond &orientation_setpoint) { + Eigen::Vector6d tau = m_controller.getFeedback( + Eigen::Vector3d::Zero(), orientation_state, velocity_state, + Eigen::Vector3d::Zero(), orientation_setpoint); return tau; } -Eigen::Vector6d Controller::orientationDepthHold(const Eigen::Vector3d& position_state, const Eigen::Quaterniond& orientation_state, - const Eigen::Vector6d& velocity_state, const Eigen::Vector3d& position_setpoint, - const Eigen::Quaterniond& orientation_setpoint) -{ - Eigen::Vector6d tau = m_controller.getFeedback(Eigen::Vector3d{0, 0, position_state[2]}, orientation_state, velocity_state, - Eigen::Vector3d{0, 0, position_setpoint[2]}, orientation_setpoint); +Eigen::Vector6d Controller::orientationDepthHold( + const Eigen::Vector3d &position_state, + const Eigen::Quaterniond &orientation_state, + const Eigen::Vector6d &velocity_state, + const Eigen::Vector3d &position_setpoint, + const Eigen::Quaterniond &orientation_setpoint) { + Eigen::Vector6d tau = m_controller.getFeedback( + Eigen::Vector3d{0, 0, position_state[2]}, orientation_state, + velocity_state, Eigen::Vector3d{0, 0, position_setpoint[2]}, + orientation_setpoint); return tau; } -void Controller::spin(){ +void Controller::spin() { ros::Rate rate(10); // Declaration of general forces Eigen::Vector6d tau_command = Eigen::VectorXd::Zero(6); geometry_msgs::Wrench tau_msg; - while( ros::ok()){ - - switch (prev_control_mode) - { - case ControlModes::OPEN_LOOP: - // let tau_command stay a zero vector - break; - - case ControlModes::POSITION_HOLD: - tau_command = positionHold(position, orientation, velocity, position_setpoint, orientation_setpoint); - tf::wrenchEigenToMsg(tau_command, tau_msg); - m_wrench_pub.publish(tau_msg); - break; - - case ControlModes::DEPTH_HOLD: - tau_command = depthHold(position, orientation, velocity, position_setpoint); - tf::wrenchEigenToMsg(tau_command, tau_msg); - m_wrench_pub.publish(tau_msg); - break; - - case ControlModes::POSITION_HEADING_HOLD: - tau_command = positionHeadingHold(position, orientation, velocity, position_setpoint, orientation_setpoint); - tf::wrenchEigenToMsg(tau_command, tau_msg); - m_wrench_pub.publish(tau_msg); - break; - - case ControlModes::HEADING_HOLD: - tau_command = headingHold(orientation, velocity, orientation_setpoint); - tf::wrenchEigenToMsg(tau_command, tau_msg); - m_wrench_pub.publish(tau_msg); - break; - - case ControlModes::POSE_HOLD: - tau_command = poseHold(position, orientation, velocity, position_setpoint, orientation_setpoint); - tf::wrenchEigenToMsg(tau_command, tau_msg); - m_wrench_pub.publish(tau_msg); - break; - - case ControlModes::DEPTH_HEADING_HOLD: { - Eigen::Vector6d tau_depthhold = depthHold(position, orientation, velocity, position_setpoint); - Eigen::Vector6d tau_headinghold = headingHold(orientation, velocity, orientation_setpoint); - tau_command = tau_depthhold + tau_headinghold; - tf::wrenchEigenToMsg(tau_command, tau_msg); - m_wrench_pub.publish(tau_msg); - break; - } - - case ControlModes::ORIENTATION_HOLD: - tau_command = orientationHold(orientation, velocity, orientation_setpoint); - tf::wrenchEigenToMsg(tau_command, tau_msg); - m_wrench_pub.publish(tau_msg); - break; - - case ControlModes::ORIENTATION_DEPTH_HOLD: - tau_command = orientationDepthHold(position, orientation, velocity, position_setpoint, orientation_setpoint); - tf::wrenchEigenToMsg(tau_command, tau_msg); - m_wrench_pub.publish(tau_msg); - break; - - default: - ROS_ERROR("Default control mode reached."); + while (ros::ok()) { + + switch (prev_control_mode) { + case ControlModes::OPEN_LOOP: + // let tau_command stay a zero vector + break; + + case ControlModes::POSITION_HOLD: + tau_command = positionHold(position, orientation, velocity, + position_setpoint, orientation_setpoint); + tf::wrenchEigenToMsg(tau_command, tau_msg); + m_wrench_pub.publish(tau_msg); + break; + + case ControlModes::DEPTH_HOLD: + tau_command = + depthHold(position, orientation, velocity, position_setpoint); + tf::wrenchEigenToMsg(tau_command, tau_msg); + m_wrench_pub.publish(tau_msg); + break; + + case ControlModes::POSITION_HEADING_HOLD: + tau_command = + positionHeadingHold(position, orientation, velocity, + position_setpoint, orientation_setpoint); + tf::wrenchEigenToMsg(tau_command, tau_msg); + m_wrench_pub.publish(tau_msg); + break; + + case ControlModes::HEADING_HOLD: + tau_command = headingHold(orientation, velocity, orientation_setpoint); + tf::wrenchEigenToMsg(tau_command, tau_msg); + m_wrench_pub.publish(tau_msg); + break; + + case ControlModes::POSE_HOLD: + tau_command = poseHold(position, orientation, velocity, position_setpoint, + orientation_setpoint); + tf::wrenchEigenToMsg(tau_command, tau_msg); + m_wrench_pub.publish(tau_msg); + break; + + case ControlModes::DEPTH_HEADING_HOLD: { + Eigen::Vector6d tau_depthhold = + depthHold(position, orientation, velocity, position_setpoint); + Eigen::Vector6d tau_headinghold = + headingHold(orientation, velocity, orientation_setpoint); + tau_command = tau_depthhold + tau_headinghold; + tf::wrenchEigenToMsg(tau_command, tau_msg); + m_wrench_pub.publish(tau_msg); + break; + } + + case ControlModes::ORIENTATION_HOLD: + tau_command = + orientationHold(orientation, velocity, orientation_setpoint); + tf::wrenchEigenToMsg(tau_command, tau_msg); + m_wrench_pub.publish(tau_msg); + break; + + case ControlModes::ORIENTATION_DEPTH_HOLD: + tau_command = + orientationDepthHold(position, orientation, velocity, + position_setpoint, orientation_setpoint); + tf::wrenchEigenToMsg(tau_command, tau_msg); + m_wrench_pub.publish(tau_msg); + break; + + default: + ROS_ERROR("Default control mode reached."); } ros::spinOnce(); diff --git a/motion/dp_controller/src/quaternion_pd_controller.cpp b/motion/dp_controller/src/quaternion_pd_controller.cpp index 5f00e3cb8..ad62d3755 100644 --- a/motion/dp_controller/src/quaternion_pd_controller.cpp +++ b/motion/dp_controller/src/quaternion_pd_controller.cpp @@ -4,9 +4,10 @@ #include "dp_controller/quaternion_pd_controller.h" -void QuaternionPdController::init(double a, double b, double c, double i, double W, double B, - const Eigen::Vector3d& r_G, const Eigen::Vector3d& r_B) -{ +void QuaternionPdController::init(double a, double b, double c, double i, + double W, double B, + const Eigen::Vector3d &r_G, + const Eigen::Vector3d &r_B) { m_r_G = r_G; m_r_B = r_B; m_W = W; @@ -14,26 +15,25 @@ void QuaternionPdController::init(double a, double b, double c, double i, double setGains(a, b, c, i); } -void QuaternionPdController::setGains(double a, double b, double c, double i) -{ +void QuaternionPdController::setGains(double a, double b, double c, double i) { m_c = c; - m_c_i = i * 0.1; // this should be chosen otherwise + m_c_i = i * 0.1; // this should be chosen otherwise m_K_d = a * Eigen::MatrixXd::Identity(6, 6); m_K_x = b * Eigen::MatrixXd::Identity(3, 3); m_K_i = i * Eigen::MatrixXd::Identity(3, 3); } -Eigen::Vector6d QuaternionPdController::getRestoring(const Eigen::Quaterniond& q) -{ +Eigen::Vector6d +QuaternionPdController::getRestoring(const Eigen::Quaterniond &q) { // Rotate from inertial/world to body Eigen::Matrix3d R = q.toRotationMatrix(); return restoringForceVector(R); } -Eigen::Vector6d QuaternionPdController::getFeedback(const Eigen::Vector3d& x, const Eigen::Quaterniond& q, - const Eigen::Vector6d& nu, const Eigen::Vector3d& x_d, - const Eigen::Quaterniond& q_d) -{ +Eigen::Vector6d QuaternionPdController::getFeedback( + const Eigen::Vector3d &x, const Eigen::Quaterniond &q, + const Eigen::Vector6d &nu, const Eigen::Vector3d &x_d, + const Eigen::Quaterniond &q_d) { // Rotate from inertial/world to body Eigen::Matrix3d R = q.toRotationMatrix(); Eigen::Matrix6d K_p = proportionalGainMatrix(R); @@ -58,68 +58,68 @@ Eigen::Vector6d QuaternionPdController::getFeedback(const Eigen::Vector3d& x, co return (Eigen::Vector6d() << gain).finished(); } -Eigen::Matrix6d QuaternionPdController::proportionalGainMatrix(const Eigen::Matrix3d R) -{ - return (Eigen::Matrix6d() << R.transpose() * m_K_x, Eigen::MatrixXd::Zero(3, 3), Eigen::MatrixXd::Zero(3, 3), +Eigen::Matrix6d +QuaternionPdController::proportionalGainMatrix(const Eigen::Matrix3d R) { + return (Eigen::Matrix6d() << R.transpose() * m_K_x, + Eigen::MatrixXd::Zero(3, 3), Eigen::MatrixXd::Zero(3, 3), m_c * Eigen::MatrixXd::Identity(3, 3)) .finished(); } -Eigen::Matrix6d QuaternionPdController::integralGainMatrix(const Eigen::Matrix3d R) -{ - return (Eigen::Matrix6d() << R.transpose() * m_K_i, Eigen::MatrixXd::Zero(3, 3), Eigen::MatrixXd::Zero(3, 3), +Eigen::Matrix6d +QuaternionPdController::integralGainMatrix(const Eigen::Matrix3d R) { + return (Eigen::Matrix6d() << R.transpose() * m_K_i, + Eigen::MatrixXd::Zero(3, 3), Eigen::MatrixXd::Zero(3, 3), m_c_i * Eigen::MatrixXd::Identity(3, 3)) .finished(); } -void QuaternionPdController::integralWindUp(Eigen::Vector6d& vec, double pose_lim, double att_lim) -{ - for (int i = 0; i < 3; i++) - { - if (vec(i) < -pose_lim || vec(i) > pose_lim) - { +void QuaternionPdController::integralWindUp(Eigen::Vector6d &vec, + double pose_lim, double att_lim) { + for (int i = 0; i < 3; i++) { + if (vec(i) < -pose_lim || vec(i) > pose_lim) { double k = copysign(pose_lim, vec(i)); vec[i] = k; } } - for (int i = 3; i < 6; i++) - { - if (vec(i) < -att_lim || vec(i) > att_lim) - { + for (int i = 3; i < 6; i++) { + if (vec(i) < -att_lim || vec(i) > att_lim) { double k = copysign(att_lim, vec(i)); vec[i] = k; } } } -Eigen::Vector6d QuaternionPdController::errorVector(const Eigen::Vector3d& x, const Eigen::Vector3d& x_d, - const Eigen::Quaterniond& q, const Eigen::Quaterniond& q_d) -{ +Eigen::Vector6d QuaternionPdController::errorVector( + const Eigen::Vector3d &x, const Eigen::Vector3d &x_d, + const Eigen::Quaterniond &q, const Eigen::Quaterniond &q_d) { Eigen::Quaterniond q_tilde = q_d.conjugate() * q; q_tilde.normalize(); Eigen::Vector3d error_body = x - x_d; - return (Eigen::Vector6d() << error_body, sgn(q_tilde.w()) * q_tilde.vec()).finished(); + return (Eigen::Vector6d() << error_body, sgn(q_tilde.w()) * q_tilde.vec()) + .finished(); } -Eigen::Vector6d QuaternionPdController::restoringForceVector(const Eigen::Matrix3d R) -{ +Eigen::Vector6d +QuaternionPdController::restoringForceVector(const Eigen::Matrix3d R) { Eigen::Vector3d f_G = R.transpose() * Eigen::Vector3d(0, 0, m_W); Eigen::Vector3d f_B = R.transpose() * Eigen::Vector3d(0, 0, -m_B); - return (Eigen::Vector6d() << f_G + f_B, m_r_G.cross(f_G) + m_r_B.cross(f_B)).finished(); + return (Eigen::Vector6d() << f_G + f_B, m_r_G.cross(f_G) + m_r_B.cross(f_B)) + .finished(); } -int QuaternionPdController::sgn(double x) -{ +int QuaternionPdController::sgn(double x) { if (x < 0) return -1; return 1; } -bool QuaternionPdController::circleOfAcceptance(const Eigen::Vector3d& x, const Eigen::Vector3d& x_d, float R) -{ +bool QuaternionPdController::circleOfAcceptance(const Eigen::Vector3d &x, + const Eigen::Vector3d &x_d, + float R) { // float R = 1.0; float distance, e_squared; @@ -130,13 +130,16 @@ bool QuaternionPdController::circleOfAcceptance(const Eigen::Vector3d& x, const return (distance < R); } -Eigen::Vector3d QuaternionPdController::referenceModel(const Eigen::Vector3d& x, const Eigen::Vector3d& x_ref) -{ +Eigen::Vector3d +QuaternionPdController::referenceModel(const Eigen::Vector3d &x, + const Eigen::Vector3d &x_ref) { Eigen::Vector3d x_d; Eigen::Vector3d a_x(1, -1.990024937655860, 0.990049813123053); - Eigen::Vector3d b_x(6.218866798092052e-06, 1.243773359618410e-05, 6.218866798092052e-06); - x_d = b_x(0) * x_ref + b_x(1) * x_ref_prev + b_x(2) * x_ref_prev_prev - a_x(1) * x_d_prev - a_x(2) * x_d_prev_prev; + Eigen::Vector3d b_x(6.218866798092052e-06, 1.243773359618410e-05, + 6.218866798092052e-06); + x_d = b_x(0) * x_ref + b_x(1) * x_ref_prev + b_x(2) * x_ref_prev_prev - + a_x(1) * x_d_prev - a_x(2) * x_d_prev_prev; // x_d[k] = x_d[k-1] x_ref_prev_prev = x_ref_prev; diff --git a/motion/dp_controller/src/setpoints.cpp b/motion/dp_controller/src/setpoints.cpp index b9d4c3835..da1dc113c 100644 --- a/motion/dp_controller/src/setpoints.cpp +++ b/motion/dp_controller/src/setpoints.cpp @@ -2,7 +2,6 @@ Copyright (c) 2019 Manta AUV, Vortex NTNU. All rights reserved. */ - #include "dp_controller/setpoints.h" Setpoints::Setpoints() { @@ -10,52 +9,47 @@ Setpoints::Setpoints() { m_orientation.setIdentity(); m_wrench_is_valid = false; - m_pose_is_valid = false; + m_pose_is_valid = false; } -bool Setpoints::getZero(Eigen::Vector6d *wrench) -{ +bool Setpoints::getZero(Eigen::Vector6d *wrench) { if (!m_wrench_is_valid) return false; - Eigen::Vector6d zero_wrench; + Eigen::Vector6d zero_wrench; zero_wrench.setZero(); *wrench = zero_wrench; - //CHANGE BY KRISTOFFER - //sreturn true; + // CHANGE BY KRISTOFFER + // sreturn true; return false; // TODO: why is this always false? } -bool Setpoints::get(Eigen::Vector3d *position, - Eigen::Quaterniond *orientation) -{ +bool Setpoints::get(Eigen::Vector3d *position, + Eigen::Quaterniond *orientation) { if (!m_pose_is_valid) return false; - *position = m_position; + *position = m_position; *orientation = m_orientation; return true; } -bool Setpoints::get(Eigen::Vector3d *position) -{ +bool Setpoints::get(Eigen::Vector3d *position) { if (!m_pose_is_valid) return false; - *position = m_position; + *position = m_position; } -bool Setpoints::get(Eigen::Quaterniond *orientation) -{ +bool Setpoints::get(Eigen::Quaterniond *orientation) { if (!m_pose_is_valid) return false; *orientation = m_orientation; } -bool Setpoints::getEuler(Eigen::Vector3d *orientation) -{ +bool Setpoints::getEuler(Eigen::Vector3d *orientation) { if (!m_pose_is_valid) return false; @@ -66,31 +60,26 @@ bool Setpoints::getEuler(Eigen::Vector3d *orientation) Eigen::Vector3d euler_orientation; - euler_orientation[0] = std::atan2(2*(w*x + y*z), 1 - 2*(x*x + y*y)); // ROLL - euler_orientation[1] = std::asin(2*(w*y - x*z)); // PITCH - euler_orientation[2] = std::atan2(2*(w*z + x*y), 1 - 2*(y*y + z*z)); // YAW + euler_orientation[0] = + std::atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y)); // ROLL + euler_orientation[1] = std::asin(2 * (w * y - x * z)); // PITCH + euler_orientation[2] = + std::atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z)); // YAW *orientation = euler_orientation; } -void Setpoints::set(const Eigen::Vector3d &position, - const Eigen::Quaterniond &orientation) -{ - m_position = position; +void Setpoints::set(const Eigen::Vector3d &position, + const Eigen::Quaterniond &orientation) { + m_position = position; m_orientation = orientation; - // if function is triggered, pose is set as true m_pose_is_valid = true; } -void Setpoints::set(const Eigen::Vector3d &position) -{ - m_position = position; -} +void Setpoints::set(const Eigen::Vector3d &position) { m_position = position; } -void Setpoints::set(const Eigen::Quaterniond &orientation) -{ +void Setpoints::set(const Eigen::Quaterniond &orientation) { m_orientation = orientation; } - diff --git a/motion/dp_controller/src/state.cpp b/motion/dp_controller/src/state.cpp index e236b9bab..3df686d04 100644 --- a/motion/dp_controller/src/state.cpp +++ b/motion/dp_controller/src/state.cpp @@ -2,59 +2,50 @@ Copyright (c) 2019 Manta AUV, Vortex NTNU. All rights reserved. */ - #include "dp_controller/state.h" -State::State() -{ +State::State() { m_position.setZero(); m_orientation.setIdentity(); m_velocity.setZero(); m_is_initialized = false; } -bool State::get(Eigen::Vector3d *position, - Eigen::Quaterniond *orientation) -{ +bool State::get(Eigen::Vector3d *position, Eigen::Quaterniond *orientation) { if (!m_is_initialized) return false; - *position = m_position; + *position = m_position; *orientation = m_orientation; return true; } -bool State::get(Eigen::Vector3d *position, - Eigen::Quaterniond *orientation, - Eigen::Vector6d *velocity) -{ +bool State::get(Eigen::Vector3d *position, Eigen::Quaterniond *orientation, + Eigen::Vector6d *velocity) { if (!m_is_initialized) return false; - *position = m_position; + *position = m_position; *orientation = m_orientation; - *velocity = m_velocity; + *velocity = m_velocity; return true; } -bool State::get(Eigen::Vector3d *position) -{ +bool State::get(Eigen::Vector3d *position) { if (!m_is_initialized) return false; *position = m_position; } -bool State::get(Eigen::Quaterniond *orientation) -{ +bool State::get(Eigen::Quaterniond *orientation) { if (!m_is_initialized) return false; *orientation = m_orientation; } -bool State::getEuler(Eigen::Vector3d *orientation) -{ +bool State::getEuler(Eigen::Vector3d *orientation) { if (!m_is_initialized) return false; @@ -65,24 +56,24 @@ bool State::getEuler(Eigen::Vector3d *orientation) Eigen::Vector3d euler_orientation; - euler_orientation[0] = std::atan2(2*(w*x + y*z), 1 - 2*(x*x + y*y)); // ROLL - euler_orientation[1] = std::asin(2*(w*y - x*z)); // PITCH in ENU - euler_orientation[2] = std::atan2(2*(w*z + x*y), 1 - 2*(y*y + z*z)); // YAW in ENU + euler_orientation[0] = + std::atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y)); // ROLL + euler_orientation[1] = std::asin(2 * (w * y - x * z)); // PITCH in ENU + euler_orientation[2] = + std::atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z)); // YAW in ENU *orientation = euler_orientation; } - -void State::set(const Eigen::Vector3d &position, +void State::set(const Eigen::Vector3d &position, const Eigen::Quaterniond &orientation, - const Eigen::Vector6d &velocity) -{ + const Eigen::Vector6d &velocity) { // convert from NED to ENU - m_position = position; + m_position = position; m_orientation = orientation; - m_velocity = velocity; + m_velocity = velocity; m_is_initialized = true; } diff --git a/motion/dp_controller/test/controller_test.cpp b/motion/dp_controller/test/controller_test.cpp index 82080e30b..4c215d943 100644 --- a/motion/dp_controller/test/controller_test.cpp +++ b/motion/dp_controller/test/controller_test.cpp @@ -1,44 +1,40 @@ #include "ros/ros.h" -#include #include +#include #include +#include #include -#include #include "vortex_msgs/PropulsionCommand.h" -class ControllerTest : public ::testing::Test -{ +class ControllerTest : public ::testing::Test { public: - ControllerTest() - { - cmdPub = nh.advertise("propulsion_command", 10); + ControllerTest() { + cmdPub = + nh.advertise("propulsion_command", 10); sub = nh.subscribe("rov_forces", 10, &ControllerTest::Callback, this); message_received = false; } - void SetUp() - { + void SetUp() { while (!IsNodeReady()) ros::spinOnce(); } - void PublishCommand(boost::array motion, std::vector mode) - { + void PublishCommand(boost::array motion, + std::vector mode) { vortex_msgs::PropulsionCommand msg; msg.motion = motion; msg.control_mode = mode; cmdPub.publish(msg); } - void ExpectTauNear(std::vector arr) - { + void ExpectTauNear(std::vector arr) { for (int i = 0; i < tau.size(); ++i) EXPECT_NEAR(tau[i], arr[i], MAX_ERROR); } - void WaitForMessage() - { + void WaitForMessage() { while (!message_received) ros::spinOnce(); } @@ -53,34 +49,27 @@ class ControllerTest : public ::testing::Test bool message_received; - void Callback(const geometry_msgs::Wrench& msg) - { + void Callback(const geometry_msgs::Wrench &msg) { tf::wrenchMsgToEigen(msg, tau); message_received = true; } - bool IsNodeReady() - { + bool IsNodeReady() { return ((cmdPub.getNumSubscribers() > 0) && (sub.getNumPublishers() > 0)); } }; -TEST_F(ControllerTest, CheckResponsiveness) -{ - WaitForMessage(); -} +TEST_F(ControllerTest, CheckResponsiveness) { WaitForMessage(); } -TEST_F(ControllerTest, OpenLoop) -{ - PublishCommand({1, 0, 0, 0, 0, 0}, {1, 0, 0, 0}); // NOLINT(whitespace/braces) +TEST_F(ControllerTest, OpenLoop) { + PublishCommand({1, 0, 0, 0, 0, 0}, {1, 0, 0, 0}); // NOLINT(whitespace/braces) ros::Duration(0.5).sleep(); WaitForMessage(); - ExpectTauNear({48.895, 0, 0, 0, 0, 0}); // NOLINT(whitespace/braces) + ExpectTauNear({48.895, 0, 0, 0, 0, 0}); // NOLINT(whitespace/braces) } -int main(int argc, char **argv) -{ +int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "controller_test"); diff --git a/motion/guidance_interface/src/guidance_interface.py b/motion/guidance_interface/src/guidance_interface.py index 8f2704290..1f5092528 100755 --- a/motion/guidance_interface/src/guidance_interface.py +++ b/motion/guidance_interface/src/guidance_interface.py @@ -93,7 +93,6 @@ def activate_joystick(self, activate): def action_server_callback(self, control_mode_goal): self.guidance_interface.stop_all_guidance() - if self.activate_joystick(True): self.joystick_controlmode_server.set_succeeded() else: @@ -103,10 +102,6 @@ def stop(self): self.activate_joystick(False) - - - - class GuidanceInterface: def __init__(self): diff --git a/motion/joystick_topside/scripts/joystick_topside.py b/motion/joystick_topside/scripts/joystick_topside.py index 21946e19b..f54d8adee 100755 --- a/motion/joystick_topside/scripts/joystick_topside.py +++ b/motion/joystick_topside/scripts/joystick_topside.py @@ -12,66 +12,78 @@ # to configure joystick environment, please refer to http://wiki.ros.org/joy/Tutorials/ConfiguringALinuxJoystick -class JoystickTopsideNode(): - - def __init__(self): - - rospy.init_node('joystick_topside') - - self.sub = rospy.Subscriber('/joystick/joy', Joy, self.callback, queue_size=1) - self.pub = rospy.Publisher('/joystick/topside_input', Joy, queue_size=1) - - self.surge_scaling = rospy.get_param('/joystick/scaling/surge') - self.sway_scaling = rospy.get_param('/joystick/scaling/sway') - self.heave_scaling = rospy.get_param('/joystick/scaling/heave') - self.roll_scaling = rospy.get_param('/joystick/scaling/roll') - self.pitch_scaling = rospy.get_param('/joystick/scaling/pitch') - self.yaw_scaling = rospy.get_param('/joystick/scaling/yaw') - - - # Name buttons and axes based on index from joy-node - self.buttons_map = ['A', 'B', 'X', 'Y', 'LB', 'RB', 'back', - 'start', 'power', 'stick_button_left', - 'stick_button_right'] - - self.axes_map = ['horizontal_axis_left_stick', - 'vertical_axis_left_stick', 'LT', - 'horizontal_axis_right_stick', - 'vertical_axis_right_stick', 'RT', - 'dpad_horizontal', 'dpad_vertical'] - - - def callback(self, msg): - - buttons = {} - axes = {} - - for i in range(len(msg.buttons)): - buttons[self.buttons_map[i]] = msg.buttons[i] - - for j in range(len(msg.axes)): - axes[self.axes_map[j]] = msg.axes[j] - - # TODO: Buttons to change control mode, should be a service server between this and guidance - - surge = axes['vertical_axis_left_stick'] * self.surge_scaling - sway = axes['horizontal_axis_left_stick'] * self.sway_scaling - heave = (axes['RT'] - axes['LT'])/2 * self.heave_scaling - roll = (buttons['RB'] - buttons['LB']) * self.roll_scaling - pitch = axes['vertical_axis_right_stick'] * self.pitch_scaling - yaw = axes['horizontal_axis_right_stick'] * self.yaw_scaling - - joystick_msg = Joy() - joystick_msg.axes = [surge, sway, heave, roll, pitch, yaw] - - self.pub.publish(joystick_msg) - - -if __name__ == '__main__': - - try: - joystick_topside = JoystickTopsideNode() - rospy.spin() - except rospy.ROSInterruptException: - pass +class JoystickTopsideNode: + def __init__(self): + + rospy.init_node("joystick_topside") + + self.sub = rospy.Subscriber("/joystick/joy", Joy, self.callback, queue_size=1) + self.pub = rospy.Publisher("/joystick/topside_input", Joy, queue_size=1) + + self.surge_scaling = rospy.get_param("/joystick/scaling/surge") + self.sway_scaling = rospy.get_param("/joystick/scaling/sway") + self.heave_scaling = rospy.get_param("/joystick/scaling/heave") + self.roll_scaling = rospy.get_param("/joystick/scaling/roll") + self.pitch_scaling = rospy.get_param("/joystick/scaling/pitch") + self.yaw_scaling = rospy.get_param("/joystick/scaling/yaw") + + # Name buttons and axes based on index from joy-node + self.buttons_map = [ + "A", + "B", + "X", + "Y", + "LB", + "RB", + "back", + "start", + "power", + "stick_button_left", + "stick_button_right", + ] + + self.axes_map = [ + "horizontal_axis_left_stick", + "vertical_axis_left_stick", + "LT", + "horizontal_axis_right_stick", + "vertical_axis_right_stick", + "RT", + "dpad_horizontal", + "dpad_vertical", + ] + + def callback(self, msg): + + buttons = {} + axes = {} + + for i in range(len(msg.buttons)): + buttons[self.buttons_map[i]] = msg.buttons[i] + + for j in range(len(msg.axes)): + axes[self.axes_map[j]] = msg.axes[j] + + # TODO: Buttons to change control mode, should be a service server between this and guidance + + surge = axes["vertical_axis_left_stick"] * self.surge_scaling + sway = axes["horizontal_axis_left_stick"] * self.sway_scaling + heave = (axes["RT"] - axes["LT"]) / 2 * self.heave_scaling + roll = (buttons["RB"] - buttons["LB"]) * self.roll_scaling + pitch = axes["vertical_axis_right_stick"] * self.pitch_scaling + yaw = axes["horizontal_axis_right_stick"] * self.yaw_scaling + + joystick_msg = Joy() + joystick_msg.axes = [surge, sway, heave, roll, pitch, yaw] + + self.pub.publish(joystick_msg) + + +if __name__ == "__main__": + + try: + joystick_topside = JoystickTopsideNode() + rospy.spin() + except rospy.ROSInterruptException: + pass diff --git a/motion/los_controller/src/backstepping/backstepping_controller.py b/motion/los_controller/src/backstepping/backstepping_controller.py index d4199ddd3..111bff2da 100755 --- a/motion/los_controller/src/backstepping/backstepping_controller.py +++ b/motion/los_controller/src/backstepping/backstepping_controller.py @@ -5,6 +5,7 @@ import numpy as np + class BacksteppingDesign: """ A class containing all the matrices, vectors and constants @@ -18,36 +19,40 @@ def __init__(self): TODO: explain physical constants """ - + # numerical values - + # model parameters # acceleration proportional - self.m = 50.000 - self.I_z = 1.9198 - self.x_g = 0.0000 + self.m = 50.000 + self.I_z = 1.9198 + self.x_g = 0.0000 self.Xu_dot = -20.7727 self.Yv_dot = -20.7727 - self.Nv_dot = 0.0000 - self.Yr_dot = 0.0000 - self.Nr_dot = 0.0000 - + self.Nv_dot = 0.0000 + self.Yr_dot = 0.0000 + self.Nr_dot = 0.0000 + # velocity proportional self.Xu = -19.5909 self.Yv = -19.5909 - self.Nv = 0.0000 - self.Yr = 0.0000 + self.Nv = 0.0000 + self.Yr = 0.0000 self.Nr = -1.1559 - + # static matrices - - self.M = np.array(( (self.m - self.Xu_dot, 0.0, 0.0), - (0.0, self.m - self.Yv_dot, self.m*self.x_g - self.Yr_dot), - (0.0, self.m*self.x_g - self.Nv_dot, self.I_z - self.Nr_dot) )) - #print(self.M) + + self.M = np.array( + ( + (self.m - self.Xu_dot, 0.0, 0.0), + (0.0, self.m - self.Yv_dot, self.m * self.x_g - self.Yr_dot), + (0.0, self.m * self.x_g - self.Nv_dot, self.I_z - self.Nr_dot), + ) + ) + # print(self.M) # nonlinear dynamics vector self.n = np.identity(3) - + def rotationMatrix(self, psi): """ Implements the 3D rotation matrix @@ -55,24 +60,28 @@ def rotationMatrix(self, psi): Args: psi Angle parameter for the rotation matrix """ - - R = np.array(( (np.cos(psi), -np.sin(psi), 0), - (np.sin(psi), np.cos(psi), 0), - ( 0, 0, 1) )) - + + R = np.array( + ((np.cos(psi), -np.sin(psi), 0), (np.sin(psi), np.cos(psi), 0), (0, 0, 1)) + ) + def nonlinVector(self, u): """ Calculate the N matrix, here given by the sum of the linearized - rigid-body coriolis and centripetal matrix and the hydrodynamic - damping matrix - + rigid-body coriolis and centripetal matrix and the hydrodynamic + damping matrix + Args: - u current velocity in the body-fixed x-direction + u current velocity in the body-fixed x-direction """ - self.n = np.array(( (-self.Xu, 0.0, 0.0), - (0.0, -self.Yv, self.m*u - self.Yr), - (0.0, -self.Nv, self.m*self.x_g*u - self.Nr) )) + self.n = np.array( + ( + (-self.Xu, 0.0, 0.0), + (0.0, -self.Yv, self.m * u - self.Yr), + (0.0, -self.Nv, self.m * self.x_g * u - self.Nr), + ) + ) class BacksteppingControl: @@ -91,33 +100,30 @@ def __init__(self, c, k1, k2, k3): k2 sway speed gain k3 heave speed gain """ - + self.bs = BacksteppingDesign() - + self.c = c - - self.K = np.array(( (k1, 0, 0), - (0, k2, 0), - (0, 0, k3) )) - - self.nu = np.transpose(np.array((0,0,0))) - self.h = np.transpose(np.array((0,0,1))) - self.alpha = np.transpose(np.array((0,0,0))) - self.alpha_dot = np.transpose(np.array((0,0,0))) - - + + self.K = np.array(((k1, 0, 0), (0, k2, 0), (0, 0, k3))) + + self.nu = np.transpose(np.array((0, 0, 0))) + self.h = np.transpose(np.array((0, 0, 1))) + self.alpha = np.transpose(np.array((0, 0, 0))) + self.alpha_dot = np.transpose(np.array((0, 0, 0))) + def updateState(self, u, u_dot, v, psi, r): """ Update the controller state values. Args: - u current velocity in the body-fixed x-direction - u_dot current acceleration in the body-fixed x-direction + u current velocity in the body-fixed x-direction + u_dot current acceleration in the body-fixed x-direction v current velocity in the body-fixed y-direction psi current heading angle in the NED frame r current angular velocity around the body-fixed z-axis """ - + # remains constant self.psi = psi self.r = r @@ -125,7 +131,6 @@ def updateState(self, u, u_dot, v, psi, r): self.u_dot = u_dot self.nu = np.transpose(np.array((u, v, r))) - def updateSetpoint(self, u_d, u_d_dot, psi_d, r_d, r_d_dot): """ Update the controller setpoint values. @@ -134,50 +139,53 @@ def updateSetpoint(self, u_d, u_d_dot, psi_d, r_d, r_d_dot): u_d desired velocity in the body-fixed x-direction u_d_dot desired acceleration in the body-fixed x-direction psi_d desired heading angle in the NED frame - r_d desired angular velocity around the body-fixed z-axis - r_d_dot desired angular acceleration around the body-fixed z-axis + r_d desired angular velocity around the body-fixed z-axis + r_d_dot desired angular acceleration around the body-fixed z-axis """ self.z1 = self.psi - psi_d self.z2 = self.nu - self.alpha - - alpha_1 = u_d #stabilizing function + + alpha_1 = u_d # stabilizing function alpha_2 = 0.0 - alpha_3 = -self.c*self.z1 + r_d #virtual control + alpha_3 = -self.c * self.z1 + r_d # virtual control self.alpha = np.transpose(np.array((alpha_1, alpha_2, alpha_3))) - - + alpha_1_dot = u_d_dot - alpha_2_dot = 0 # dissapears - alpha_3_dot = -self.c*(self.r-r_d) + r_d_dot + alpha_2_dot = 0 # dissapears + alpha_3_dot = -self.c * (self.r - r_d) + r_d_dot self.alpha_dot = np.transpose(np.array((alpha_1_dot, alpha_2_dot, alpha_3_dot))) - - + def controlLaw(self, u, u_dot, u_d, u_d_dot, v, psi, psi_d, r, r_d, r_d_dot): """ Update states and setpoints, then calculate the control vector Args: - u current velocity in the body-fixed x-direction - u_dot current acceleration in the body-fixed x-direction - u_d desired velocity in the body-fixed x-direction - u_d_dot desired acceleration in the body-fixed x-direction - v current velocity in the body-fixed y-direction - psi current heading angle in the NED frame - psi_d desired heading angle in the NED frame + u current velocity in the body-fixed x-direction + u_dot current acceleration in the body-fixed x-direction + u_d desired velocity in the body-fixed x-direction + u_d_dot desired acceleration in the body-fixed x-direction + v current velocity in the body-fixed y-direction + psi current heading angle in the NED frame + psi_d desired heading angle in the NED frame r current angular velocity around the body-fixed z-axis - r_d desired angular velocity around the body-fixed z-axis - r_d_dot desired angular acceleration around the body-fixed z-axis + r_d desired angular velocity around the body-fixed z-axis + r_d_dot desired angular acceleration around the body-fixed z-axis - Returns: - float[3]: The control force vector tau + Returns: + float[3]: The control force vector tau """ - + self.updateState(u, u_dot, v, psi, r) - + self.updateSetpoint(u_d, u_d_dot, psi_d, r_d, r_d_dot) - + # control force - tau = self.bs.M.dot(self.alpha_dot) + self.bs.n.dot(self.nu) - self.K.dot(self.z2) - self.h.dot(self.z1) + tau = ( + self.bs.M.dot(self.alpha_dot) + + self.bs.n.dot(self.nu) + - self.K.dot(self.z2) + - self.h.dot(self.z1) + ) return tau diff --git a/motion/los_controller/src/los_controller_node.py b/motion/los_controller/src/los_controller_node.py index 30e68cfb8..a03d63104 100755 --- a/motion/los_controller/src/los_controller_node.py +++ b/motion/los_controller/src/los_controller_node.py @@ -15,345 +15,348 @@ from dynamic_reconfigure.server import Server from los_controller.cfg import LOSControllerConfig -class LOSControllerPID: - """ - Wrapper for the PID controller to make the los_controller - code cleaner. - """ - - def __init__(self): - """ - Initialize the PID controller with fixed gains and saturation limit. - """ - self.controller = PIDRegulator(25, 0.024, 3.5, 5.0) # Args: p, i, d, sat +class LOSControllerPID: + """ + Wrapper for the PID controller to make the los_controller + code cleaner. + """ + def __init__(self): + """ + Initialize the PID controller with fixed gains and saturation limit. + """ - def updateGains(self, p, i, d, sat): - """ - Update the controller gains and saturation limit. + self.controller = PIDRegulator(25, 0.024, 3.5, 5.0) # Args: p, i, d, sat - Args: - p proportional gain - i integral gain - d derivative gain - sat saturation limit - """ + def updateGains(self, p, i, d, sat): + """ + Update the controller gains and saturation limit. - self.controller.p = p - self.controller.i = i - self.controller.d = d - self.controller.sat = sat + Args: + p proportional gain + i integral gain + d derivative gain + sat saturation limit + """ + self.controller.p = p + self.controller.i = i + self.controller.d = d + self.controller.sat = sat - def headingController(self, psi_d, psi, t): - """ - Calculate force to maintain fixed heading. + def headingController(self, psi_d, psi, t): + """ + Calculate force to maintain fixed heading. - Args: - psi_d desired heading - psi current heading - t time + Args: + psi_d desired heading + psi current heading + t time - Returns: - float: A restoring force output by the controller. - """ + Returns: + float: A restoring force output by the controller. + """ - # error ENU - e_rot = psi_d - psi + # error ENU + e_rot = psi_d - psi - # regulate(err, t) - tau = self.controller.regulate(e_rot, t) - return tau + # regulate(err, t) + tau = self.controller.regulate(e_rot, t) + return tau + def depthController(self, z_d, z, t): + """ + Calculate force to maintain fixed depth. - def depthController(self, z_d, z, t): - """ - Calculate force to maintain fixed depth. + Args: + z_d desired depth + z current depth + t time - Args: - z_d desired depth - z current depth - t time + Returns: + float: A restoring force output by the controller. + """ - Returns: - float: A restoring force output by the controller. - """ + e = z_d - z - e = z_d - z + tau = self.controller.regulate(e, t) + return tau - tau = self.controller.regulate(e, t) - return tau # Not in use? class CameraPID: - """ - (0,0) increase-> - ----------------> X - | - | - | increase - | | - | v - v + """ + (0,0) increase-> + ----------------> X + | + | + | increase + | | + | v + v - Y + Y - """ + """ - def __init__(self): + def __init__(self): - self.sway = PIDRegulator(0.01, 0.0001, 0.0, 7.5) - #self.heading = PIDRegulator(0.02, 0.0, 0.0, 0.15) - self.heading = PIDRegulator(25,0.002, 0.0, 100) - self.depth = PIDRegulator(25, 0.024, 3.5, 5.0) - self.speed = PIDRegulator(25, 0.024, 3.5, 5.0) + self.sway = PIDRegulator(0.01, 0.0001, 0.0, 7.5) + # self.heading = PIDRegulator(0.02, 0.0, 0.0, 0.15) + self.heading = PIDRegulator(25, 0.002, 0.0, 100) + self.depth = PIDRegulator(25, 0.024, 3.5, 5.0) + self.speed = PIDRegulator(25, 0.024, 3.5, 5.0) - def swayController(self, px_d, px, t): + def swayController(self, px_d, px, t): - # error - e = px_d - px + # error + e = px_d - px - tau = self.sway.regulate(e, t) + tau = self.sway.regulate(e, t) - return tau + return tau - def depthController(self, z_d, z, t): + def depthController(self, z_d, z, t): - e = z_d - z + e = z_d - z - tau = self.depth.regulate(e, t) + tau = self.depth.regulate(e, t) - return tau + return tau + def speedController(self, u_d, u, t): - def speedController(self, u_d, u, t): + e = u_d - u - e = u_d - u + tau = self.speed.regulate(e, t) - tau = self.speed.regulate(e, t) + return tau - return tau + def headingController(self, psi_d, psi, t): - def headingController(self, psi_d, psi, t): + # error ENU + e_rot = psi_d - psi - # error ENU - e_rot = psi_d - psi + # regulate(err, t) + tau = self.heading.regulate(e_rot, t) - # regulate(err, t) - tau = self.heading.regulate(e_rot, t) + return tau - return tau class LOSControllerBackstepping: - """ - Wrapper for the backstepping controller. - """ - - def __init__(self): - """ - Initialize the backstepping controller with fixed parameters. - """ - # 0.75, 30, 12, 2.5 - self.controller = BacksteppingControl(3.75, 45.0, 28.0, 10.5) - - - def updateGains(self, c, k1, k2, k3): - """ - Update the backstepping controller gains. + """ + Wrapper for the backstepping controller. + """ + + def __init__(self): + """ + Initialize the backstepping controller with fixed parameters. + """ + # 0.75, 30, 12, 2.5 + self.controller = BacksteppingControl(3.75, 45.0, 28.0, 10.5) + + def updateGains(self, c, k1, k2, k3): + """ + Update the backstepping controller gains. Args: c a double containing heading gain k1 a double containing surge speed gain k2 a double containing sway speed gain k3 a double containing heave speed gain - """ - - self.controller.c = c - self.controller.K = np.array(( (k1, 0, 0), - (0, k2, 0), - (0, 0, k3) )) - - - def regulate(self, u, u_dot, u_d, u_d_dot, v, psi, psi_d, r, r_d, r_d_dot): - """ - A wrapper for the controlLaw() method in the BacksteppingContoller - class, to make the los_controller code cleaner. - - Args: - u current velocity in the body-fixed x-direction - u_dot current acceleration in the body-fixed x-direction - u_d desired velocity in the body-fixed x-direction - u_d_dot desired acceleration in the body-fixed x-direction - v current velocity in the body-fixed y-direction - psi current heading angle in the NED frame - psi_d desired heading angle in the NED frame - r current angular velocity around the body-fixed z-axis - r_d desired angular velocity around the body-fixed z-axis - r_d_dot desired angular acceleration around the body-fixed z-axis - - Returns: - float[3]: The control force vector tau - """ - - tau = self.controller.controlLaw(u, u_dot, u_d, u_d_dot, v, psi, psi_d, r, r_d, r_d_dot) - return tau + """ + + self.controller.c = c + self.controller.K = np.array(((k1, 0, 0), (0, k2, 0), (0, 0, k3))) + + def regulate(self, u, u_dot, u_d, u_d_dot, v, psi, psi_d, r, r_d, r_d_dot): + """ + A wrapper for the controlLaw() method in the BacksteppingContoller + class, to make the los_controller code cleaner. + Args: + u current velocity in the body-fixed x-direction + u_dot current acceleration in the body-fixed x-direction + u_d desired velocity in the body-fixed x-direction + u_d_dot desired acceleration in the body-fixed x-direction + v current velocity in the body-fixed y-direction + psi current heading angle in the NED frame + psi_d desired heading angle in the NED frame + r current angular velocity around the body-fixed z-axis + r_d desired angular velocity around the body-fixed z-axis + r_d_dot desired angular acceleration around the body-fixed z-axis + + Returns: + float[3]: The control force vector tau + """ + + tau = self.controller.controlLaw( + u, u_dot, u_d, u_d_dot, v, psi, psi_d, r, r_d, r_d_dot + ) + return tau class LOSController: - """ - The ROS wrapper class for the LOSController. The los_controller is made up - of a PID and a backstepping controller, and is mainly used in - conjunction with the LOS guidance system. + """ + The ROS wrapper class for the LOSController. The los_controller is made up + of a PID and a backstepping controller, and is mainly used in + conjunction with the LOS guidance system. + + Nodes created: + los_controller + + Subscribes to: + /guidance/los_data + + Publishes to: + /thrust/desired_forces - Nodes created: - los_controller + """ - Subscribes to: - /guidance/los_data + def __init__(self): + """ + Initialize the los_controller node, subscribers, publishers and the + objects for the PID and the backstepping controllers. + """ - Publishes to: - /thrust/desired_forces + rospy.init_node("los_controller") - """ + thrust_topic = rospy.get_param( + "/controllers/los/thrust_topic", default="/thrust/desired_forces" + ) - def __init__(self): - """ - Initialize the los_controller node, subscribers, publishers and the - objects for the PID and the backstepping controllers. - """ + # Create controllers + self.Backstepping = LOSControllerBackstepping() + self.PID = LOSControllerPID() - rospy.init_node('los_controller') - - thrust_topic = rospy.get_param("/controllers/los/thrust_topic", default="/thrust/desired_forces") - - # Create controllers - self.Backstepping = LOSControllerBackstepping() - self.PID = LOSControllerPID() - - # Subscribers - self.sub_reference_model = rospy.Subscriber('/reference_model/los_data', GuidanceData, self.reference_model_data_callback, queue_size=1) - - # Publishers - self.pub_thrust = rospy.Publisher(thrust_topic, Wrench, queue_size=1) - - # Dynamic reconfigure - self.config = {} - self.srv_reconfigure = Server(LOSControllerConfig, self.config_callback) - - def reference_model_data_callback(self, msg): - """ - Handle guidance data whenever it is published by calculating - a control vector based on the given data. - - Args: - msg: The guidance data message - """ + # Subscribers + self.sub_reference_model = rospy.Subscriber( + "/reference_model/los_data", + GuidanceData, + self.reference_model_data_callback, + queue_size=1, + ) - # Control forces - tau_d = self.Backstepping.regulate( - msg.u, - msg.u_dot, - msg.u_d, - msg.u_d_dot, - msg.v, - msg.psi, - msg.psi_d, - msg.r, - msg.r_d, - msg.r_d_dot) - - tau_depth_hold = self.PID.depthController(msg.z_d, msg.z, msg.t) - - # add speed controllers here - - thrust_msg = Wrench() - - # Thrust message forces and torque - if tau_d[0] > 0.0: - thrust_msg.force.x = tau_d[0] - - thrust_msg.force.y = tau_d[1] - thrust_msg.force.z = tau_depth_hold - - thrust_msg.torque.z = tau_d[2] - - # Publish the thrust message to /auv/thruster_manager/input - self.pub_thrust.publish(thrust_msg) - - def log_value_if_updated(self, name, old_value, new_value): - """ - A helper function for the config_callback() method - - Args: - name The string name of the variable - old_value A real number - new_value A real number - """ - - if old_value != new_value: - rospy.loginfo("\t {:}: {:.4f} -> {:.4f}".format(name, old_value, new_value)) - - - def config_callback(self, config, level): - """ - Handle updated configuration values. - - Args: - config The dynamic reconfigure server's Config type variable - level Ununsed variable - - Returns: - A Config type containing the updated config argument. - """ - - # Old parameters - p_old = self.PID.controller.p - i_old = self.PID.controller.i - d_old = self.PID.controller.d - sat_old = self.PID.controller.sat - - c_old = self.Backstepping.controller.c - K = self.Backstepping.controller.K - - # Reconfigured PID parameters - p = config['PID_p'] - i = config['PID_i'] - d = config['PID_d'] - sat = config['PID_sat'] - - # Reconfigured Backstepping parameters - c = config['Backstepping_c'] - k1 = config['Backstepping_k1'] - k2 = config['Backstepping_k2'] - k3 = config['Backstepping_k3'] - - rospy.loginfo("los_controller reconfigure: ") - - self.log_value_if_updated('p', p_old, p) - self.log_value_if_updated('i', i_old, i) - self.log_value_if_updated('d', d_old, d) - self.log_value_if_updated('sat', sat_old, sat) - - self.log_value_if_updated('c', c_old, c) - self.log_value_if_updated('k1', K[0, 0], k1) - self.log_value_if_updated('k2', K[1, 1], k2) - self.log_value_if_updated('k3', K[2, 2], k3) - - # Update controller gains - self.PID.updateGains(p, i, d, sat) - self.Backstepping.updateGains(c, k1, k2, k3) - - # update config - self.config = config - - return config - - -if __name__ == '__main__': - try: - los_controller = LOSController() - rospy.spin() - except rospy.ROSInterruptException: - pass + # Publishers + self.pub_thrust = rospy.Publisher(thrust_topic, Wrench, queue_size=1) + + # Dynamic reconfigure + self.config = {} + self.srv_reconfigure = Server(LOSControllerConfig, self.config_callback) + + def reference_model_data_callback(self, msg): + """ + Handle guidance data whenever it is published by calculating + a control vector based on the given data. + + Args: + msg: The guidance data message + """ + + # Control forces + tau_d = self.Backstepping.regulate( + msg.u, + msg.u_dot, + msg.u_d, + msg.u_d_dot, + msg.v, + msg.psi, + msg.psi_d, + msg.r, + msg.r_d, + msg.r_d_dot, + ) + + tau_depth_hold = self.PID.depthController(msg.z_d, msg.z, msg.t) + + # add speed controllers here + + thrust_msg = Wrench() + + # Thrust message forces and torque + if tau_d[0] > 0.0: + thrust_msg.force.x = tau_d[0] + + thrust_msg.force.y = tau_d[1] + thrust_msg.force.z = tau_depth_hold + + thrust_msg.torque.z = tau_d[2] + + # Publish the thrust message to /auv/thruster_manager/input + self.pub_thrust.publish(thrust_msg) + + def log_value_if_updated(self, name, old_value, new_value): + """ + A helper function for the config_callback() method + + Args: + name The string name of the variable + old_value A real number + new_value A real number + """ + + if old_value != new_value: + rospy.loginfo("\t {:}: {:.4f} -> {:.4f}".format(name, old_value, new_value)) + + def config_callback(self, config, level): + """ + Handle updated configuration values. + + Args: + config The dynamic reconfigure server's Config type variable + level Ununsed variable + + Returns: + A Config type containing the updated config argument. + """ + + # Old parameters + p_old = self.PID.controller.p + i_old = self.PID.controller.i + d_old = self.PID.controller.d + sat_old = self.PID.controller.sat + + c_old = self.Backstepping.controller.c + K = self.Backstepping.controller.K + + # Reconfigured PID parameters + p = config["PID_p"] + i = config["PID_i"] + d = config["PID_d"] + sat = config["PID_sat"] + + # Reconfigured Backstepping parameters + c = config["Backstepping_c"] + k1 = config["Backstepping_k1"] + k2 = config["Backstepping_k2"] + k3 = config["Backstepping_k3"] + + rospy.loginfo("los_controller reconfigure: ") + + self.log_value_if_updated("p", p_old, p) + self.log_value_if_updated("i", i_old, i) + self.log_value_if_updated("d", d_old, d) + self.log_value_if_updated("sat", sat_old, sat) + + self.log_value_if_updated("c", c_old, c) + self.log_value_if_updated("k1", K[0, 0], k1) + self.log_value_if_updated("k2", K[1, 1], k2) + self.log_value_if_updated("k3", K[2, 2], k3) + + # Update controller gains + self.PID.updateGains(p, i, d, sat) + self.Backstepping.updateGains(c, k1, k2, k3) + + # update config + self.config = config + + return config + + +if __name__ == "__main__": + try: + los_controller = LOSController() + rospy.spin() + except rospy.ROSInterruptException: + pass diff --git a/motion/los_controller/src/pid/pid_controller.py b/motion/los_controller/src/pid/pid_controller.py index f32ea5070..91ab46ca6 100755 --- a/motion/los_controller/src/pid/pid_controller.py +++ b/motion/los_controller/src/pid/pid_controller.py @@ -5,71 +5,72 @@ import numpy as np + class PIDRegulator: - """ - A very basic 1D PID controller - """ - - def __init__(self, p, i, d, sat): - """ - Initialize the PID controller by setting gains - and saturation - - Args: - p proportional gain - i integral gain - d derivative gain - sat saturation limit - """ - - self.p = p - self.i = i - self.d = d - self.sat = sat - - self.integral = 0 - self.prev_err = 0 - self.prev_t = -1 - - def __str__(self): - """ - Create a formatted string containing the controller gains - and the saturation limit. - """ - - msg = 'PID controller:' - msg += '\n\tp=%f' % self.p - msg += '\n\cd ti=%f' % self.i - msg += '\n\td=%f' % self.d - msg += '\n\tsat=%f' % self.sat - return msg - - def regulate(self, err, t): - """ - Calculate the controller gain - - Args: - err the state error used to calculate controller gain (e) - t the current time - - Returns: - float: The controller gain u - """ - - derr_dt = 0.0 - dt = t - self.prev_t - if self.prev_t > 0.0 and dt > 0.0: - derr_dt = (err - self.prev_err)/dt - self.integral += 0.5*(err + self.prev_err)*dt - - u = self.p*err + self.d*derr_dt + self.i*self.integral - - self.prev_err = err - self.prev_t = t - - if (np.linalg.norm(u) > self.sat): - # controller is in saturation: limit output, reset integral - u = self.sat*u/np.linalg.norm(u) - self.integral = 0.0 - - return u + """ + A very basic 1D PID controller + """ + + def __init__(self, p, i, d, sat): + """ + Initialize the PID controller by setting gains + and saturation + + Args: + p proportional gain + i integral gain + d derivative gain + sat saturation limit + """ + + self.p = p + self.i = i + self.d = d + self.sat = sat + + self.integral = 0 + self.prev_err = 0 + self.prev_t = -1 + + def __str__(self): + """ + Create a formatted string containing the controller gains + and the saturation limit. + """ + + msg = "PID controller:" + msg += "\n\tp=%f" % self.p + msg += "\n\cd ti=%f" % self.i + msg += "\n\td=%f" % self.d + msg += "\n\tsat=%f" % self.sat + return msg + + def regulate(self, err, t): + """ + Calculate the controller gain + + Args: + err the state error used to calculate controller gain (e) + t the current time + + Returns: + float: The controller gain u + """ + + derr_dt = 0.0 + dt = t - self.prev_t + if self.prev_t > 0.0 and dt > 0.0: + derr_dt = (err - self.prev_err) / dt + self.integral += 0.5 * (err + self.prev_err) * dt + + u = self.p * err + self.d * derr_dt + self.i * self.integral + + self.prev_err = err + self.prev_t = t + + if np.linalg.norm(u) > self.sat: + # controller is in saturation: limit output, reset integral + u = self.sat * u / np.linalg.norm(u) + self.integral = 0.0 + + return u diff --git a/motion/los_guidance/scripts/los_guidance_node.py b/motion/los_guidance/scripts/los_guidance_node.py index 2ba966236..50eea3690 100755 --- a/motion/los_guidance/scripts/los_guidance_node.py +++ b/motion/los_guidance/scripts/los_guidance_node.py @@ -18,432 +18,456 @@ # action message import actionlib -from vortex_msgs.msg import LosPathFollowingAction, LosPathFollowingGoal, LosPathFollowingResult, LosPathFollowingFeedback +from vortex_msgs.msg import ( + LosPathFollowingAction, + LosPathFollowingGoal, + LosPathFollowingResult, + LosPathFollowingFeedback, +) + class LOS: - """ - The Line-Of-Sight guidance class, with an imported controller. + """ + The Line-Of-Sight guidance class, with an imported controller. - Physical attributes referenced in the class: - x, y, z: surge, sway, heave (position) - u, v, w: surge, sway, heave (velocity) + Physical attributes referenced in the class: + x, y, z: surge, sway, heave (position) + u, v, w: surge, sway, heave (velocity) + + alpha: The path-tangential angle + psi: Heading angle required to reach the LOS intersection + point. - alpha: The path-tangential angle - psi: Heading angle required to reach the LOS intersection - point. + R: sphere of acceptance. If the AUV is inside the sphere + defined by this radius and the setpoint, it will be + considered to have reached the setpoint. + """ + + def __init__(self): + + # update rate + self.h = 0.05 + self.u = 0.0 + + # current position + self.x = 0.0 + self.y = 0.0 + self.z = 0.0 + + # previous waypoint + self.x_k = 0.0 + self.y_k = 0.0 + + # next waypoint + self.x_kp1 = 0.0 + self.y_kp1 = 0.0 - R: sphere of acceptance. If the AUV is inside the sphere - defined by this radius and the setpoint, it will be - considered to have reached the setpoint. - """ + # depth hold depth + self.z_d = 0.0 - def __init__(self): + # desired speed + self.speed = 0 - # update rate - self.h = 0.05 - self.u = 0.0 + # sphere of acceptance + self.R = 0.5 - # current position - self.x = 0.0 - self.y = 0.0 - self.z = 0.0 + # look-ahead distance + Lpp = 0.7 + self.delta = 1.0 * Lpp - # previous waypoint - self.x_k = 0.0 - self.y_k = 0.0 + def updateState(self, x, y, z, u, v, w, psi, r, time): + """ + Update all state values contained in the LOS class. - # next waypoint - self.x_kp1 = 0.0 - self.y_kp1 = 0.0 + Args: + x Surge; position in the direction of the x-axis. + y Sway; position in the direction of the y-axis. + z Heave; position in the direction of the z-axis. - # depth hold depth - self.z_d = 0.0 + u Body fixed velocity in the x-direction. + v Body fixed velocity in the y-direction. + w Body fixed velocity in the z-direction. - # desired speed - self.speed = 0 + psi Heading angle required to reach the LOS intersection + point. + r current angular velocity around the body-fixed z-axis + time A double with the current time + """ - # sphere of acceptance - self.R = 0.5 + # Update position + self.x = x + self.y = y + self.z = z + + # Update velocities + self.u_dot = (u - self.u) / self.h + self.u = u + self.v = v + self.w = w - # look-ahead distance - Lpp = 0.7 - self.delta = 1.0*Lpp + self.psi = psi + self.r = r + self.t = time + def setWayPoints(self, x_k, y_k, x_kp1, y_kp1): + """ + Set the previous and next waypoints - def updateState(self, x, y, z, u, v, w, psi, r, time): - """ - Update all state values contained in the LOS class. + Args: + x_k x-component of the previous waypoint + y_k y-component of the previous waypoint - Args: - x Surge; position in the direction of the x-axis. - y Sway; position in the direction of the y-axis. - z Heave; position in the direction of the z-axis. + x_kp1 x-component of the next waypoint + y_kp1 y-component of the next waypoint + """ - u Body fixed velocity in the x-direction. - v Body fixed velocity in the y-direction. - w Body fixed velocity in the z-direction. + # previous waypoint + self.x_k = x_k + self.y_k = y_k - psi Heading angle required to reach the LOS intersection - point. - r current angular velocity around the body-fixed z-axis - time A double with the current time - """ + # next waypoint + self.x_kp1 = x_kp1 + self.y_kp1 = y_kp1 - # Update position - self.x = x - self.y = y - self.z = z - - # Update velocities - self.u_dot = (u - self.u) / self.h - self.u = u - self.v = v - self.w = w - - self.psi = psi - self.r = r - self.t = time - - def setWayPoints(self, x_k, y_k, x_kp1, y_kp1): - """ - Set the previous and next waypoints + def distance(self): + """ + Calculate straight line distance (2D) between the + current position and the setpoint position. + """ - Args: - x_k x-component of the previous waypoint - y_k y-component of the previous waypoint + return np.sqrt((self.x_kp1 - self.x) ** 2 + (self.y_kp1 - self.y) ** 2) - x_kp1 x-component of the next waypoint - y_kp1 y-component of the next waypoint - """ + def sphereOfAcceptance(self): + """ + The sphere of acceptance is a sphere around the setpoint. + If the AUV is inside this sphere, it will be considered + as having reached the setpoint. - # previous waypoint - self.x_k = x_k - self.y_k = y_k + Returns: + bool: True if the current position is less than the + radius of the sphere of acceptance. False otherwise + """ - # next waypoint - self.x_kp1 = x_kp1 - self.y_kp1 = y_kp1 + return self.distance() < self.R - def distance(self): - """ - Calculate straight line distance (2D) between the - current position and the setpoint position. - """ + def getEpsilonVector(self): + """ + Calculate the epsilon vector, which is the vector + that contains the coordinates of the AUV in the + path-fixed reference frame for a straight line going + from the reference point to the target position. - return np.sqrt((self.x_kp1 - self.x)**2 + - (self.y_kp1 - self.y)**2 ) + Returns: + float: The calculated epsilon vector + """ - def sphereOfAcceptance(self): - """ - The sphere of acceptance is a sphere around the setpoint. - If the AUV is inside this sphere, it will be considered - as having reached the setpoint. + alpha = self.alpha - Returns: - bool: True if the current position is less than the - radius of the sphere of acceptance. False otherwise - """ + # rotation matrix + R = np.array(((np.cos(alpha), -np.sin(alpha)), (np.sin(alpha), np.cos(alpha)))) - return self.distance() < self.R + # transpose + R_T = np.transpose(R) + # position vector + p_t = np.array((self.x, self.y)) + p_k = np.array((self.x_k, self.y_k)) - def getEpsilonVector(self): - """ - Calculate the epsilon vector, which is the vector - that contains the coordinates of the AUV in the - path-fixed reference frame for a straight line going - from the reference point to the target position. + # epsilon (eq 10.56 Fossen) + epsilon = R_T.dot(p_t - p_k) - Returns: - float: The calculated epsilon vector + return epsilon - """ + def quat2euler(self, msg): + """ + Calculate roll, pitch and yaw from the orientation + quaternion with the axis sequence xyzw - alpha = self.alpha + Args: + msg A nav_msgs/Odometry message - # rotation matrix - R = np.array(( (np.cos(alpha), -np.sin(alpha)), - (np.sin(alpha), np.cos(alpha)) )) + Returns: + float: The euler yaw angle calculated from the msg argument + """ - # transpose - R_T = np.transpose(R) + global roll, pitch, yaw + orientation_q = msg.pose.pose.orientation + orientation_list = [ + orientation_q.x, + orientation_q.y, + orientation_q.z, + orientation_q.w, + ] + (roll, pitch, yaw) = euler_from_quaternion(orientation_list) - # position vector - p_t = np.array((self.x, self.y)) - p_k = np.array((self.x_k, self.y_k)) + return yaw - # epsilon (eq 10.56 Fossen) - epsilon = R_T.dot(p_t - p_k) + def lookaheadBasedSteering(self): + """ + Calculate the desired heading angle. This angle is + the sum of the path-tangential angle and the velocity- + path relative angle. - return epsilon + Returns: + float: The desired heading angle chi_d + """ - def quat2euler(self,msg): - """ - Calculate roll, pitch and yaw from the orientation - quaternion with the axis sequence xyzw + # straight-line path segment + self.y_delta = self.y_kp1 - self.y_k + self.x_delta = self.x_kp1 - self.x_k - Args: - msg A nav_msgs/Odometry message + # angle + self.alpha = np.arctan2(self.y_delta, self.x_delta) - Returns: - float: The euler yaw angle calculated from the msg argument - """ + # rotation matrix + epsilon = self.getEpsilonVector() - global roll, pitch, yaw - orientation_q = msg.pose.pose.orientation - orientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w] - (roll,pitch,yaw) = euler_from_quaternion(orientation_list) + # along track distance + self.s = epsilon[0] - return yaw + # cross-track error + self.e = epsilon[1] - def lookaheadBasedSteering(self): - """ - Calculate the desired heading angle. This angle is - the sum of the path-tangential angle and the velocity- - path relative angle. + # path-tangential angle (eq 10.73 Fossen) + self.chi_p = self.alpha - Returns: - float: The desired heading angle chi_d - """ + # velocity-path relative angle (eq 10.74 Fossen) + self.chi_r = np.arctan(-self.e / self.delta) - # straight-line path segment - self.y_delta = self.y_kp1 - self.y_k - self.x_delta = self.x_kp1 - self.x_k + # desired heading angle + self.chi_d = self.chi_p + self.chi_r - # angle - self.alpha = np.arctan2(self.y_delta, self.x_delta) + return self.chi_d - # rotation matrix - epsilon = self.getEpsilonVector() - # along track distance - self.s = epsilon[0] +class LosPathFollowing(object): + """ + This is the ROS wrapper class for the LOS class. - # cross-track error - self.e = epsilon[1] + Attributes: + _feedback A vortex_msgs action that contains the distance to goal + _result A vortex_msgs action, true if a goal is set within the + sphereof acceptance, false if not - # path-tangential angle (eq 10.73 Fossen) - self.chi_p = self.alpha + Nodes created: + los - # velocity-path relative angle (eq 10.74 Fossen) - self.chi_r = np.arctan(-self.e / self.delta) + Subscribes to: + /odometry/filtered - # desired heading angle - self.chi_d = self.chi_p + self.chi_r + Publishes to: + /guidance/los_data + /auv/los_desired + """ - return self.chi_d + # create messages that are used to send feedback/result + _feedback = LosPathFollowingFeedback() + _result = LosPathFollowingResult() -class LosPathFollowing(object): - """ - This is the ROS wrapper class for the LOS class. - - Attributes: - _feedback A vortex_msgs action that contains the distance to goal - _result A vortex_msgs action, true if a goal is set within the - sphereof acceptance, false if not - - Nodes created: - los - - Subscribes to: - /odometry/filtered - - Publishes to: - /guidance/los_data - /auv/los_desired - """ - - # create messages that are used to send feedback/result - _feedback = LosPathFollowingFeedback() - _result = LosPathFollowingResult() - - def __init__(self): - """ - To initialize the ROS wrapper, the node, subscribers - and publishers are set up, as well as dynamic - reconfigure and action servers. - """ + def __init__(self): + """ + To initialize the ROS wrapper, the node, subscribers + and publishers are set up, as well as dynamic + reconfigure and action servers. + """ - """ + """ A flag to indicate whether or not a goal has not been reached. True means that a goal is in progress of being completed. False means that a goal has been completed (or not started with any goal) """ - rospy.init_node('los') - - self.publish_guidance_data = False - - # parameters - odom_topic = rospy.get_param("/guidance/los/odom_topic", default="/odometry/filtered") - rate = rospy.get_param("/guidance/los/rate", default=40) - self.ros_rate = rospy.Rate(rate) - - # constructor object - self.los = LOS() - - # dynamic reconfigure - self.config = {} - self.srv_reconfigure = Server(LOSConfig, self.config_cb) - - # Publishers - self.pub_data_los_controller = rospy.Publisher('/guidance/los_data', GuidanceData, queue_size=1) - - # Action server, see https://github.com/strawlab/ros_common/blob/master/actionlib/src/actionlib/simple_action_server.py - self.action_server = actionlib.SimpleActionServer(name='los_action_server', ActionSpec=LosPathFollowingAction, auto_start=False) - self.action_server.register_goal_callback(self.goal_cb) - self.action_server.register_preempt_callback(self.preempt_cb) - self.action_server.start() - - # Subscribers - self.sub = rospy.Subscriber(odom_topic, Odometry, self.odometry_cb, queue_size=1) # 20hz - rospy.loginfo("Waiting for initial odometry..") - rospy.wait_for_message(odom_topic, Odometry) - - rospy.loginfo("los guidance initiated") - - def spin(self): - - while not rospy.is_shutdown(): - if self.publish_guidance_data is True: - #Publish guidance data to the reference model - guidance_data = GuidanceData() - - guidance_data.u = self.los.u - guidance_data.u_dot = self.los.u_dot - - guidance_data.psi = self.psi - - guidance_data.r = self.los.r - - guidance_data.z = self.los.z - guidance_data.z_d = self.los.z_d - - guidance_data.v = self.los.v - guidance_data.t = self.los.t - - guidance_data.psi_ref = self.psi_ref - guidance_data.speed = self.los.speed - - self.pub_data_los_controller.publish(guidance_data) - - # check if action goal succeeded - self.statusActionGoal() - - self.ros_rate.sleep() - - def odometry_cb(self, msg): - """ - The callback used in the subscribed topic /odometry/filtered. - When called, position and velocity states are updated, and - a new current goal is set. + rospy.init_node("los") - If the self.publish_guidance_data attribute is True, we have not yet reached a goal - and so a control force is published, alongside the desired - pose. + self.publish_guidance_data = False + + # parameters + odom_topic = rospy.get_param( + "/guidance/los/odom_topic", default="/odometry/filtered" + ) + rate = rospy.get_param("/guidance/los/rate", default=40) + self.ros_rate = rospy.Rate(rate) + + # constructor object + self.los = LOS() + + # dynamic reconfigure + self.config = {} + self.srv_reconfigure = Server(LOSConfig, self.config_cb) + + # Publishers + self.pub_data_los_controller = rospy.Publisher( + "/guidance/los_data", GuidanceData, queue_size=1 + ) + + # Action server, see https://github.com/strawlab/ros_common/blob/master/actionlib/src/actionlib/simple_action_server.py + self.action_server = actionlib.SimpleActionServer( + name="los_action_server", + ActionSpec=LosPathFollowingAction, + auto_start=False, + ) + self.action_server.register_goal_callback(self.goal_cb) + self.action_server.register_preempt_callback(self.preempt_cb) + self.action_server.start() + + # Subscribers + self.sub = rospy.Subscriber( + odom_topic, Odometry, self.odometry_cb, queue_size=1 + ) # 20hz + rospy.loginfo("Waiting for initial odometry..") + rospy.wait_for_message(odom_topic, Odometry) + + rospy.loginfo("los guidance initiated") + + def spin(self): + + while not rospy.is_shutdown(): + if self.publish_guidance_data is True: + # Publish guidance data to the reference model + guidance_data = GuidanceData() + + guidance_data.u = self.los.u + guidance_data.u_dot = self.los.u_dot + + guidance_data.psi = self.psi + + guidance_data.r = self.los.r + + guidance_data.z = self.los.z + guidance_data.z_d = self.los.z_d + + guidance_data.v = self.los.v + guidance_data.t = self.los.t + + guidance_data.psi_ref = self.psi_ref + guidance_data.speed = self.los.speed + + self.pub_data_los_controller.publish(guidance_data) + + # check if action goal succeeded + self.statusActionGoal() + + self.ros_rate.sleep() + + def odometry_cb(self, msg): + """ + The callback used in the subscribed topic /odometry/filtered. + When called, position and velocity states are updated, and + a new current goal is set. + + If the self.publish_guidance_data attribute is True, we have not yet reached a goal + and so a control force is published, alongside the desired + pose. + + Args: + msg A nav_msgs/Odometry ROS message type + """ - Args: - msg A nav_msgs/Odometry ROS message type - """ + # update current position + self.psi = self.los.quat2euler(msg) - # update current position - self.psi = self.los.quat2euler(msg) + # update position and velocities + self.los.updateState( + msg.pose.pose.position.x, + msg.pose.pose.position.y, + msg.pose.pose.position.z, + msg.twist.twist.linear.x, + msg.twist.twist.linear.y, + msg.twist.twist.linear.z, + self.psi, + msg.twist.twist.angular.z, + msg.header.stamp.to_sec(), + ) - # update position and velocities - self.los.updateState(msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z, - msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z, - self.psi, msg.twist.twist.angular.z, msg.header.stamp.to_sec()) + # update current goal + self.psi_ref = self.los.lookaheadBasedSteering() + + def statusActionGoal(self): + """ + Publish the current distance to target and check if the current + position is within the sphere of acceptance. If it is, the + attribute _result can be set to true, and the flag to false. + """ - # update current goal - self.psi_ref = self.los.lookaheadBasedSteering() - - def statusActionGoal(self): - """ - Publish the current distance to target and check if the current - position is within the sphere of acceptance. If it is, the - attribute _result can be set to true, and the flag to false. - """ - - # feedback - self._feedback.distanceToGoal = self.los.distance() - self.action_server.publish_feedback(self._feedback) + # feedback + self._feedback.distanceToGoal = self.los.distance() + self.action_server.publish_feedback(self._feedback) - # succeeded - if self.los.sphereOfAcceptance(): - self._result.terminalSector = True - self.action_server.set_succeeded(self._result, text="goal completed") - self.publish_guidance_data = False + # succeeded + if self.los.sphereOfAcceptance(): + self._result.terminalSector = True + self.action_server.set_succeeded(self._result, text="goal completed") + self.publish_guidance_data = False - def preempt_cb(self): - """ - The preempt callback for the action server. - """ + def preempt_cb(self): + """ + The preempt callback for the action server. + """ - # check that preempt has not been requested by the client - if self.action_server.is_preempt_requested(): - rospy.loginfo("Preempted requested by los path client") - self.action_server.set_preempted() - self.publish_guidance_data = False + # check that preempt has not been requested by the client + if self.action_server.is_preempt_requested(): + rospy.loginfo("Preempted requested by los path client") + self.action_server.set_preempted() + self.publish_guidance_data = False - def goal_cb(self): - """ - The goal callback for the action server. - - Once a goal has been recieved from the client, self.publish_guidance_data is set to True - This means that this node will start publishing data for the controller - """ + def goal_cb(self): + """ + The goal callback for the action server. - _goal = self.action_server.accept_new_goal() - rospy.logdebug("los_guidance recieved new goal") + Once a goal has been recieved from the client, self.publish_guidance_data is set to True + This means that this node will start publishing data for the controller + """ - # set goal - self.los.x_k = self.los.x - self.los.y_k = self.los.y - self.los.x_kp1 = _goal.next_waypoint.x - self.los.y_kp1 = _goal.next_waypoint.y + _goal = self.action_server.accept_new_goal() + rospy.logdebug("los_guidance recieved new goal") - # forward speed - self.los.speed = _goal.forward_speed - - # depth hold - self.los.z_d = _goal.desired_depth - - # sphere of acceptance - self.los.R = _goal.sphereOfAcceptance - - self.publish_guidance_data = True - - def config_cb(self, config, level): - """ - Handle updated configuration values. - - Args: - config The dynamic reconfigure server's config - level Ununsed variable - - Returns: - The updated config argument. - """ + # set goal + self.los.x_k = self.los.x + self.los.y_k = self.los.y + self.los.x_kp1 = _goal.next_waypoint.x + self.los.y_kp1 = _goal.next_waypoint.y - delta = config['delta'] - - # Print reconfigure data with precision of 4 decimal points. - rospy.loginfo("los_guidance reconfigure:") - rospy.loginfo("\t delta: {:.4f} -> {:.4f}".format(self.los.delta, delta)) - - # update look-ahead distance and config - self.los.delta = config['delta'] - self.config = config + # forward speed + self.los.speed = _goal.forward_speed - return config + # depth hold + self.los.z_d = _goal.desired_depth + # sphere of acceptance + self.los.R = _goal.sphereOfAcceptance + self.publish_guidance_data = True -if __name__ == '__main__': - try: - los_path_following = LosPathFollowing() - los_path_following.spin() + def config_cb(self, config, level): + """ + Handle updated configuration values. - except rospy.ROSInterruptException: - pass + Args: + config The dynamic reconfigure server's config + level Ununsed variable + + Returns: + The updated config argument. + """ + + delta = config["delta"] + + # Print reconfigure data with precision of 4 decimal points. + rospy.loginfo("los_guidance reconfigure:") + rospy.loginfo("\t delta: {:.4f} -> {:.4f}".format(self.los.delta, delta)) + + # update look-ahead distance and config + self.los.delta = config["delta"] + self.config = config + + return config + + +if __name__ == "__main__": + try: + los_path_following = LosPathFollowing() + los_path_following.spin() + + except rospy.ROSInterruptException: + pass diff --git a/motion/reference_model/dp_reference_model/include/dp_reference_model/reference_model.h b/motion/reference_model/dp_reference_model/include/dp_reference_model/reference_model.h index f72fa8065..49c1373c0 100644 --- a/motion/reference_model/dp_reference_model/include/dp_reference_model/reference_model.h +++ b/motion/reference_model/dp_reference_model/include/dp_reference_model/reference_model.h @@ -16,10 +16,10 @@ #include #include -#include "ros/ros.h" +#include "eigen_conversions/eigen_msg.h" #include "geometry_msgs/Point.h" #include "geometry_msgs/Pose.h" -#include "eigen_conversions/eigen_msg.h" +#include "ros/ros.h" #include "tf/tf.h" #include "tf_conversions/tf_eigen.h" @@ -27,8 +27,7 @@ using namespace Eigen; -class ReferenceModel -{ +class ReferenceModel { private: int prev_control_mode; @@ -40,19 +39,21 @@ class ReferenceModel Eigen::Vector3d b_x; /* Positions */ - Eigen::Vector3d x_d_prev; /** Previous desired body position */ - Eigen::Vector3d x_d_prev_prev; /** Previous previous desired body position */ - Eigen::Vector3d x_ref_prev; /** Previous reference body position */ - Eigen::Vector3d x_ref_prev_prev; /** Previous previous reference body position */ + Eigen::Vector3d x_d_prev; /** Previous desired body position */ + Eigen::Vector3d + x_d_prev_prev; /** Previous previous desired body position */ + Eigen::Vector3d x_ref_prev; /** Previous reference body position */ + Eigen::Vector3d + x_ref_prev_prev; /** Previous previous reference body position */ /** * @brief Calculate and publish the desired, smooth position * and orientation. * * - * @param setpoint_msg target setpoint + * @param setpoint_msg target setpoint */ - void setpoint_cb(const vortex_msgs::DpSetpoint& setpoint_msg); + void setpoint_cb(const vortex_msgs::DpSetpoint &setpoint_msg); /** * @brief Utility function that calculates a smooth trajectory from current @@ -60,7 +61,7 @@ class ReferenceModel * * @param x_ref Reference used */ - Eigen::Vector3d calculate_smooth(const Eigen::Vector3d& x_ref); + Eigen::Vector3d calculate_smooth(const Eigen::Vector3d &x_ref); /** * @brief Low pass filters the reference value @@ -68,7 +69,7 @@ class ReferenceModel * @param x_ref reference value * @return x_d */ - Eigen::Vector3d low_pass_filter(const Eigen::Vector3d& x_ref); + Eigen::Vector3d low_pass_filter(const Eigen::Vector3d &x_ref); /** * @brief Function that resets the private variables to @p pos @@ -85,8 +86,9 @@ class ReferenceModel */ ReferenceModel(ros::NodeHandle nh); - ros::Subscriber setpoint_sub; /* Subscriber for listening to the guidance node */ - ros::Publisher reference_pub; /* Publisher for the DP-controller */ + ros::Subscriber + setpoint_sub; /* Subscriber for listening to the guidance node */ + ros::Publisher reference_pub; /* Publisher for the DP-controller */ }; -#endif // DP_REFERENCE_MODEL_H \ No newline at end of file +#endif // DP_REFERENCE_MODEL_H \ No newline at end of file diff --git a/motion/reference_model/dp_reference_model/src/reference_model.cpp b/motion/reference_model/dp_reference_model/src/reference_model.cpp index ecd56bc6d..73e910d19 100644 --- a/motion/reference_model/dp_reference_model/src/reference_model.cpp +++ b/motion/reference_model/dp_reference_model/src/reference_model.cpp @@ -5,8 +5,7 @@ #include "dp_reference_model/reference_model.h" -ReferenceModel::ReferenceModel(ros::NodeHandle nh) -{ +ReferenceModel::ReferenceModel(ros::NodeHandle nh) { // get params double a1, a2, a3; double b1, b2, b3; @@ -22,37 +21,38 @@ ReferenceModel::ReferenceModel(ros::NodeHandle nh) b1 = 6.218866798092052e-06; if (!nh.getParam("dp_rm/b2", b2)) b2 = 1.243773359618410e-05; - if (!nh.getParam("dp_rm/b3", b3)) + if (!nh.getParam("dp_rm/b3", b3)) b3 = 6.218866798092052e-06; if (!nh.getParam("/reference_model/dp/beta", beta_temp)) - beta_temp = { 0.025, 0.025, 0.0025 }; + beta_temp = {0.025, 0.025, 0.0025}; a_x = Eigen::Vector3d(a1, a2, a3); b_x = Eigen::Vector3d(b1, b2, b3); beta = Eigen::Vector3d(beta_temp[0], beta_temp[1], beta_temp[2]); // initiate local variables - prev_control_mode = 0; // OPEN LOOP + prev_control_mode = 0; // OPEN LOOP x_d_prev = Eigen::Vector3d::Zero(); x_d_prev_prev = Eigen::Vector3d::Zero(); x_ref_prev = Eigen::Vector3d::Zero(); x_ref_prev_prev = Eigen::Vector3d::Zero(); // subs and pubs - setpoint_sub = nh.subscribe("/guidance/dp_data", 1, &ReferenceModel::setpoint_cb, this); - reference_pub = nh.advertise("/reference_model/output", 1, this); + setpoint_sub = + nh.subscribe("/guidance/dp_data", 1, &ReferenceModel::setpoint_cb, this); + reference_pub = + nh.advertise("/reference_model/output", 1, this); } -void ReferenceModel::setpoint_cb(const vortex_msgs::DpSetpoint& setpoint_msg) -{ +void ReferenceModel::setpoint_cb(const vortex_msgs::DpSetpoint &setpoint_msg) { // parse msg - Eigen::Vector3d x_ref{ setpoint_msg.setpoint.position.x, setpoint_msg.setpoint.position.y, - setpoint_msg.setpoint.position.z }; - + Eigen::Vector3d x_ref{setpoint_msg.setpoint.position.x, + setpoint_msg.setpoint.position.y, + setpoint_msg.setpoint.position.z}; + // check if control mode has changed - if (setpoint_msg.control_mode != prev_control_mode) - { + if (setpoint_msg.control_mode != prev_control_mode) { reset(x_ref); // reset prev values to current target position prev_control_mode = setpoint_msg.control_mode; ROS_DEBUG("DP reference model reset"); @@ -73,20 +73,18 @@ void ReferenceModel::setpoint_cb(const vortex_msgs::DpSetpoint& setpoint_msg) reference_pub.publish(dp_setpoint); } -Eigen::Vector3d ReferenceModel::low_pass_filter(const Eigen::Vector3d& x_ref) -{ +Eigen::Vector3d ReferenceModel::low_pass_filter(const Eigen::Vector3d &x_ref) { Eigen::Vector3d x_d; - for (int i = 0; i < 3; i++) - { + for (int i = 0; i < 3; i++) { x_d[i] = x_d_prev[i] - beta[i] * (x_d_prev[i] - x_ref[i]); } return x_d; } -Eigen::Vector3d ReferenceModel::calculate_smooth(const Eigen::Vector3d& x_ref) -{ +Eigen::Vector3d ReferenceModel::calculate_smooth(const Eigen::Vector3d &x_ref) { Eigen::Vector3d x_d; - x_d = b_x(0) * x_ref + b_x(1) * x_ref_prev + b_x(2) * x_ref_prev_prev - a_x(1) * x_d_prev - a_x(2) * x_d_prev_prev; + x_d = b_x(0) * x_ref + b_x(1) * x_ref_prev + b_x(2) * x_ref_prev_prev - + a_x(1) * x_d_prev - a_x(2) * x_d_prev_prev; x_ref_prev_prev = x_ref_prev; x_ref_prev = x_ref; @@ -96,8 +94,7 @@ Eigen::Vector3d ReferenceModel::calculate_smooth(const Eigen::Vector3d& x_ref) return x_d; } -void ReferenceModel::reset(Eigen::Vector3d pos) -{ +void ReferenceModel::reset(Eigen::Vector3d pos) { x_d_prev = pos; x_d_prev_prev = pos; x_ref_prev = pos; diff --git a/motion/reference_model/dp_reference_model/src/reference_model_node.cpp b/motion/reference_model/dp_reference_model/src/reference_model_node.cpp index 92a4d00d7..a4ee4e118 100644 --- a/motion/reference_model/dp_reference_model/src/reference_model_node.cpp +++ b/motion/reference_model/dp_reference_model/src/reference_model_node.cpp @@ -2,10 +2,10 @@ #include "dp_reference_model/reference_model.h" int main(int argc, char *argv[]) { - ros::init(argc, argv, "dp_reference_mode"); - ros::NodeHandle nh; - ReferenceModel referenceModel(nh); - ros::spin(); + ros::init(argc, argv, "dp_reference_mode"); + ros::NodeHandle nh; + ReferenceModel referenceModel(nh); + ros::spin(); - return 0; + return 0; } diff --git a/motion/reference_model/los_reference_model/scripts/discrete_tustin.py b/motion/reference_model/los_reference_model/scripts/discrete_tustin.py index 83dd71634..ef6b0512b 100755 --- a/motion/reference_model/los_reference_model/scripts/discrete_tustin.py +++ b/motion/reference_model/los_reference_model/scripts/discrete_tustin.py @@ -6,99 +6,111 @@ import rospy import numpy as np import time -class ReferenceModel: - def __init__(self, x, h): - - """ - x = [] - x_d[k] = b0*x_ref*[k] + b1*x_ref*[k - 1] + b2*x_ref*[k - 2] - a1*x_d*[k-1] - a_2*x_d*[k - 2] - """ - self.h = h - # determines position of poles - self.a0 = 1.0 - self.a1 = -1.9312 - self.a2 = 0.9324 - - # determine zeros - self.b0 = 2.9581e-04 - self.b1 = 5.9161e-04 - self.b2 = 2.9581e-04 - - # set intial values - self.x_ref_prev_prev = x # 2nd-to-last raw input value - self.x_ref_prev = x # last raw input value - self.x_ref = x # current raw input value - self.x_d_prev_prev = x # 2nd-to-last filtered (output) value - self.x_d_prev = x # last filtered (output) value - self.x_d = x # current filtered (output value) - - # set initial values for derivates - self.x_d_dot = 0 - self.x_d_dot_prev = 0 - self.x_d_dot_dot = 0 - - def resetFilter(self, x): - - # set intial values - self.x_ref_prev_prev = x # 2nd-to-last raw input value - self.x_ref_prev = x # last raw input value - self.x_ref = x # current raw input value - self.x_d_prev_prev = x # 2nd-to-last filtered (output) value - self.x_d_prev = x # last filtered (output) value - self.x_d = x # current filtered (output value) - - # set initial values for derivates - self.x_d_dot = 0 - self.x_d_dot_prev = 0 - self.x_d_dot_dot = 0 - - def computeDerivatives(self): - # new setpoint - # [u_d_dot, r_d] - self.x_d_dot = (self.x_d - self.x_d_prev)/self.h - - # [u_d_dot_dot, r_d_dot] - self.x_d_dot_dot = (self.x_d_dot - self.x_d_dot_prev)/self.h - - # update prev - self.x_d_dot_prev = self.x_d_dot - - # [u_d, u_d_dot, psi_d, r_d, r_d_dot] - x_output = np.array((self.x_d[0], - self.x_d_dot[0], - self.x_d[1], - self.x_d_dot[1], - self.x_d_dot_dot[1])) - return x_output - - def discreteTustinMSD(self, x_ref): - - self.x_ref = x_ref - - # new set point - self.x_d = self.b0*self.x_ref + self.b1*self.x_ref_prev + self.b2*self.x_ref_prev_prev - self.a1 * self.x_d_prev - self.a2 * self.x_d_prev_prev - - # include velocities and accelerations - self.x_output = self.computeDerivatives() - - # update - self.x_ref_prev_prev = self.x_ref_prev - self.x_ref_prev = self.x_ref - self.x_d_prev_prev = self.x_d_prev - self.x_d_prev = self.x_d - - return self.x_output - -if __name__ == '__main__': - - x = np.array((0.5, 0.0)) - x_ref = np.array((2.0,1.57)) - - rm = ReferenceModel(x, 0.05) - count = 0 - - while (count < 400): - time.sleep(.05) - xd = rm.discreteTustinMSD(x_ref) - print(xd) + +class ReferenceModel: + def __init__(self, x, h): + + """ + x = [] + x_d[k] = b0*x_ref*[k] + b1*x_ref*[k - 1] + b2*x_ref*[k - 2] - a1*x_d*[k-1] - a_2*x_d*[k - 2] + """ + self.h = h + # determines position of poles + self.a0 = 1.0 + self.a1 = -1.9312 + self.a2 = 0.9324 + + # determine zeros + self.b0 = 2.9581e-04 + self.b1 = 5.9161e-04 + self.b2 = 2.9581e-04 + + # set intial values + self.x_ref_prev_prev = x # 2nd-to-last raw input value + self.x_ref_prev = x # last raw input value + self.x_ref = x # current raw input value + self.x_d_prev_prev = x # 2nd-to-last filtered (output) value + self.x_d_prev = x # last filtered (output) value + self.x_d = x # current filtered (output value) + + # set initial values for derivates + self.x_d_dot = 0 + self.x_d_dot_prev = 0 + self.x_d_dot_dot = 0 + + def resetFilter(self, x): + + # set intial values + self.x_ref_prev_prev = x # 2nd-to-last raw input value + self.x_ref_prev = x # last raw input value + self.x_ref = x # current raw input value + self.x_d_prev_prev = x # 2nd-to-last filtered (output) value + self.x_d_prev = x # last filtered (output) value + self.x_d = x # current filtered (output value) + + # set initial values for derivates + self.x_d_dot = 0 + self.x_d_dot_prev = 0 + self.x_d_dot_dot = 0 + + def computeDerivatives(self): + # new setpoint + # [u_d_dot, r_d] + self.x_d_dot = (self.x_d - self.x_d_prev) / self.h + + # [u_d_dot_dot, r_d_dot] + self.x_d_dot_dot = (self.x_d_dot - self.x_d_dot_prev) / self.h + + # update prev + self.x_d_dot_prev = self.x_d_dot + + # [u_d, u_d_dot, psi_d, r_d, r_d_dot] + x_output = np.array( + ( + self.x_d[0], + self.x_d_dot[0], + self.x_d[1], + self.x_d_dot[1], + self.x_d_dot_dot[1], + ) + ) + return x_output + + def discreteTustinMSD(self, x_ref): + + self.x_ref = x_ref + + # new set point + self.x_d = ( + self.b0 * self.x_ref + + self.b1 * self.x_ref_prev + + self.b2 * self.x_ref_prev_prev + - self.a1 * self.x_d_prev + - self.a2 * self.x_d_prev_prev + ) + + # include velocities and accelerations + self.x_output = self.computeDerivatives() + + # update + self.x_ref_prev_prev = self.x_ref_prev + self.x_ref_prev = self.x_ref + self.x_d_prev_prev = self.x_d_prev + self.x_d_prev = self.x_d + + return self.x_output + + +if __name__ == "__main__": + + x = np.array((0.5, 0.0)) + x_ref = np.array((2.0, 1.57)) + + rm = ReferenceModel(x, 0.05) + count = 0 + + while count < 400: + time.sleep(0.05) + xd = rm.discreteTustinMSD(x_ref) + print(xd) diff --git a/motion/reference_model/los_reference_model/scripts/los_reference_model_node.py b/motion/reference_model/los_reference_model/scripts/los_reference_model_node.py index a16d4b0ac..3085f87a0 100755 --- a/motion/reference_model/los_reference_model/scripts/los_reference_model_node.py +++ b/motion/reference_model/los_reference_model/scripts/los_reference_model_node.py @@ -9,102 +9,107 @@ from vortex_msgs.msg import GuidanceData -class LOSReferenceModelNode(): - """ - This is the ROS wrapper class for the reference model class. - - Nodes created: - los_reference_model - - Subscribes to: - /guidance/los_data - - Publishes to: - /reference_model/los_data - """ - def __init__(self): - # Update rate - self.h = 0.05 - self.u = 0.0 - - # ROS node init - rospy.init_node('rm_los') - - # Subscribers - self.guidance_data_sub = rospy.Subscriber('/guidance/los_data', GuidanceData, self.guidanceDataCallback, queue_size=1) - - # Publishers - self.rm_los_data_pub = rospy.Publisher('/reference_model/los_data', GuidanceData, queue_size=1) - - # RM object - self.reference_model = ReferenceModel(np.array((0, 0)), self.h) - - def fixHeadingWrapping(self, u, psi, psi_ref, speed): - """ - The heading angle is obtained by the use of an arctangent - function, which is discontinuous at -pi and pi. This can - be problematic when the heading angle is fed into the - reference model. This function fixes this problem by - wrapping the angles around by 2pi. - """ - - e = psi - psi_ref - if e < -math.pi: - psi_ref = psi_ref - 2*math.pi - if e > math.pi: - psi_ref = psi_ref + 2*math.pi - - - # Reference Model - x_d = self.reference_model.discreteTustinMSD(np.array((speed, psi_ref))) - psi_d = x_d[2] - - e = psi - psi_d - if e > math.pi: - psi_d = psi_d - 2*math.pi - self.reference_model = ReferenceModel(np.array((u, psi)), self.h) - x_d = self.reference_model.discreteTustinMSD(np.array((speed, psi_d))) - if e < -math.pi: - psi_d = psi_d + 2*math.pi - self.reference_model = ReferenceModel(np.array((u, psi)), self.h) - x_d = self.reference_model.discreteTustinMSD(np.array((speed, psi_d))) - return x_d - - def guidanceDataCallback(self, msg): - """ - Gets topic msg from LOS guidance module, applies discrete Tustin transform, - publishes data from guidance and the calculated data to LOS controller. - """ - - # Guidance data sub - u = msg.u - psi = msg.psi - psi_ref = msg.psi_ref - speed = msg.speed - - # Reference model calc - """ +class LOSReferenceModelNode: + """ + This is the ROS wrapper class for the reference model class. + + Nodes created: + los_reference_model + + Subscribes to: + /guidance/los_data + + Publishes to: + /reference_model/los_data + """ + + def __init__(self): + # Update rate + self.h = 0.05 + self.u = 0.0 + + # ROS node init + rospy.init_node("rm_los") + + # Subscribers + self.guidance_data_sub = rospy.Subscriber( + "/guidance/los_data", GuidanceData, self.guidanceDataCallback, queue_size=1 + ) + + # Publishers + self.rm_los_data_pub = rospy.Publisher( + "/reference_model/los_data", GuidanceData, queue_size=1 + ) + + # RM object + self.reference_model = ReferenceModel(np.array((0, 0)), self.h) + + def fixHeadingWrapping(self, u, psi, psi_ref, speed): + """ + The heading angle is obtained by the use of an arctangent + function, which is discontinuous at -pi and pi. This can + be problematic when the heading angle is fed into the + reference model. This function fixes this problem by + wrapping the angles around by 2pi. + """ + + e = psi - psi_ref + if e < -math.pi: + psi_ref = psi_ref - 2 * math.pi + if e > math.pi: + psi_ref = psi_ref + 2 * math.pi + + # Reference Model + x_d = self.reference_model.discreteTustinMSD(np.array((speed, psi_ref))) + psi_d = x_d[2] + + e = psi - psi_d + if e > math.pi: + psi_d = psi_d - 2 * math.pi + self.reference_model = ReferenceModel(np.array((u, psi)), self.h) + x_d = self.reference_model.discreteTustinMSD(np.array((speed, psi_d))) + if e < -math.pi: + psi_d = psi_d + 2 * math.pi + self.reference_model = ReferenceModel(np.array((u, psi)), self.h) + x_d = self.reference_model.discreteTustinMSD(np.array((speed, psi_d))) + return x_d + + def guidanceDataCallback(self, msg): + """ + Gets topic msg from LOS guidance module, applies discrete Tustin transform, + publishes data from guidance and the calculated data to LOS controller. + """ + + # Guidance data sub + u = msg.u + psi = msg.psi + psi_ref = msg.psi_ref + speed = msg.speed + + # Reference model calc + """ Wrapping would have been avoided by using quaternions instead of Euler angles if you don't care about wrapping, use this instead: x_d = self.reference_model.discreteTustinMSD(np.array((self.los.speed,psi_d))) """ - x_d = self.fixHeadingWrapping(u, psi, psi_ref, speed) + x_d = self.fixHeadingWrapping(u, psi, psi_ref, speed) + + # Reference Model data pub - # Reference Model data pub + msg.u_d = x_d[0] + msg.u_d_dot = x_d[1] + msg.psi_d = x_d[2] + msg.r_d = x_d[3] + msg.r_d_dot = x_d[4] - msg.u_d = x_d[0] - msg.u_d_dot = x_d[1] - msg.psi_d = x_d[2] - msg.r_d = x_d[3] - msg.r_d_dot = x_d[4] + self.rm_los_data_pub.publish(msg) - self.rm_los_data_pub.publish(msg) -if __name__ == '__main__': - try: - rm_los_node = LOSReferenceModelNode() +if __name__ == "__main__": + try: + rm_los_node = LOSReferenceModelNode() - rospy.spin() - except rospy.ROSInterruptException: - pass + rospy.spin() + except rospy.ROSInterruptException: + pass diff --git a/motion/thrust_merger/include/thrust_merger/thrust_merger.h b/motion/thrust_merger/include/thrust_merger/thrust_merger.h index 53aed2879..813e4b41d 100644 --- a/motion/thrust_merger/include/thrust_merger/thrust_merger.h +++ b/motion/thrust_merger/include/thrust_merger/thrust_merger.h @@ -3,22 +3,20 @@ #include -#include -#include #include +#include +#include #include #include // These typdefs are lacking from the default eigen namespace -namespace Eigen -{ +namespace Eigen { typedef Eigen::Matrix Matrix6d; typedef Eigen::Matrix Vector6d; -} // namespace Eigen +} // namespace Eigen -class ThrustMerger -{ +class ThrustMerger { public: ThrustMerger(ros::NodeHandle nh); void spin(); diff --git a/motion/thrust_merger/src/thrust_merger.cpp b/motion/thrust_merger/src/thrust_merger.cpp index b69db511d..44beb349d 100644 --- a/motion/thrust_merger/src/thrust_merger.cpp +++ b/motion/thrust_merger/src/thrust_merger.cpp @@ -1,7 +1,6 @@ #include "thrust_merger/thrust_merger.h" -ThrustMerger::ThrustMerger(ros::NodeHandle nh) : nh(nh) -{ +ThrustMerger::ThrustMerger(ros::NodeHandle nh) : nh(nh) { // parameters std::string dp_topic; std::string los_topic; @@ -21,8 +20,6 @@ ThrustMerger::ThrustMerger(ros::NodeHandle nh) : nh(nh) joy_topic = "/thrust/joy"; if (!nh.getParam("/thrust_merger/output_topic", output_topic)) output_topic = "/thrust/combined"; - - // init wrenches as zero dp_wrench = Eigen::Vector6d(); @@ -47,11 +44,9 @@ ThrustMerger::ThrustMerger(ros::NodeHandle nh) : nh(nh) ROS_INFO("thrust_merger initiated"); } -void ThrustMerger::spin() -{ +void ThrustMerger::spin() { ros::Rate ros_rate(rate); - while (ros::ok()) - { + while (ros::ok()) { // execute waiting callbacks ros::spinOnce(); @@ -86,26 +81,22 @@ void ThrustMerger::spin() } } -void ThrustMerger::dpCallback(geometry_msgs::Wrench wrench_msgs) -{ +void ThrustMerger::dpCallback(geometry_msgs::Wrench wrench_msgs) { tf::wrenchMsgToEigen(wrench_msgs, dp_wrench); dp_counter = 0; } -void ThrustMerger::losCallback(geometry_msgs::Wrench wrench_msgs) -{ +void ThrustMerger::losCallback(geometry_msgs::Wrench wrench_msgs) { tf::wrenchMsgToEigen(wrench_msgs, los_wrench); los_counter = 0; } -void ThrustMerger::velCallback(geometry_msgs::Wrench wrench_msgs) -{ +void ThrustMerger::velCallback(geometry_msgs::Wrench wrench_msgs) { tf::wrenchMsgToEigen(wrench_msgs, vel_wrench); vel_counter = 0; } -void ThrustMerger::joyCallback(geometry_msgs::Wrench wrench_msgs) -{ +void ThrustMerger::joyCallback(geometry_msgs::Wrench wrench_msgs) { tf::wrenchMsgToEigen(wrench_msgs, joy_wrench); joy_counter = 0; } diff --git a/motion/thrust_merger/src/thrust_merger_node.cpp b/motion/thrust_merger/src/thrust_merger_node.cpp index 3335d52f3..d0e6300bb 100644 --- a/motion/thrust_merger/src/thrust_merger_node.cpp +++ b/motion/thrust_merger/src/thrust_merger_node.cpp @@ -1,15 +1,14 @@ #include "thrust_merger/thrust_merger.h" -int main(int argc, char** argv) -{ - const bool DEBUG_MODE = false; // debug logs are printed to console when true +int main(int argc, char **argv) { + const bool DEBUG_MODE = false; // debug logs are printed to console when true ros::init(argc, argv, "thrust_merger"); ros::NodeHandle nh; - if (DEBUG_MODE) - { - ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug); + if (DEBUG_MODE) { + ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, + ros::console::levels::Debug); ros::console::notifyLoggerLevelsChanged(); } diff --git a/motion/thruster_allocator/include/vortex_allocator/allocator_ros.h b/motion/thruster_allocator/include/vortex_allocator/allocator_ros.h index 480f49807..dc65a263d 100644 --- a/motion/thruster_allocator/include/vortex_allocator/allocator_ros.h +++ b/motion/thruster_allocator/include/vortex_allocator/allocator_ros.h @@ -5,30 +5,28 @@ #include #include -#include #include +#include #include #include -#include "vortex_allocator/eigen_typedefs.h" #include "vortex_allocator/eigen_helper.h" +#include "vortex_allocator/eigen_typedefs.h" #include "vortex_allocator/pseudoinverse_allocator.h" - -class Allocator -{ +class Allocator { public: explicit Allocator(ros::NodeHandle nh); void callback(const geometry_msgs::Wrench &msg) const; void thrusterForcesCb(const std_msgs::Float32MultiArray &thruster_forces_msg); - + private: ros::NodeHandle m_nh; // for computing thruster forces from body forces ros::Subscriber m_sub; - ros::Publisher m_pub; + ros::Publisher m_pub; // for publishing delivered thrust in body ros::Subscriber thruster_forces_sub; @@ -47,4 +45,4 @@ class Allocator Eigen::VectorXd rovForcesMsgToEigen(const geometry_msgs::Wrench &msg) const; }; -#endif // VORTEX_ALLOCATOR_ALLOCATOR_ROS_H +#endif // VORTEX_ALLOCATOR_ALLOCATOR_ROS_H diff --git a/motion/thruster_allocator/include/vortex_allocator/eigen_helper.h b/motion/thruster_allocator/include/vortex_allocator/eigen_helper.h index 46a1bf685..cd83efa93 100644 --- a/motion/thruster_allocator/include/vortex_allocator/eigen_helper.h +++ b/motion/thruster_allocator/include/vortex_allocator/eigen_helper.h @@ -1,17 +1,16 @@ #ifndef VORTEX_EIGEN_HELPER_H #define VORTEX_EIGEN_HELPER_H -#include "std_msgs/Float32MultiArray.h" #include "ros/ros.h" +#include "std_msgs/Float32MultiArray.h" #include #include #include // Return false if X has any nan or inf elements. -template -inline bool isFucked(const Eigen::MatrixBase& X) -{ +template +inline bool isFucked(const Eigen::MatrixBase &X) { bool has_nan = !(X.array() == X.array()).all(); bool has_inf = !((X - X).array() == (X - X).array()).all(); return has_nan || has_inf; @@ -19,29 +18,25 @@ inline bool isFucked(const Eigen::MatrixBase& X) // Read a matrix from the ROS parameter server. // Return false if unsuccessful. -inline bool getMatrixParam(ros::NodeHandle nh, std::string name, Eigen::MatrixXd *X) -{ +inline bool getMatrixParam(ros::NodeHandle nh, std::string name, + Eigen::MatrixXd *X) { XmlRpc::XmlRpcValue matrix; nh.getParam(name, matrix); - try - { + try { const int rows = matrix.size(); const int cols = matrix[0].size(); X->setZero(rows, cols); for (int i = 0; i < rows; ++i) for (int j = 0; j < cols; ++j) (*X)(i, j) = matrix[i][j]; - } - catch(...) - { + } catch (...) { return false; } return true; } -inline void printEigen(std::string name, const Eigen::MatrixXd &X) -{ +inline void printEigen(std::string name, const Eigen::MatrixXd &X) { std::stringstream ss; ss << std::endl << name << " = " << std::endl << X; ROS_INFO_STREAM(ss.str()); @@ -49,12 +44,10 @@ inline void printEigen(std::string name, const Eigen::MatrixXd &X) // Calculate the pseudoinverse matrix of the matrix X. // Return false if the calculations fails. -inline bool pseudoinverse(const Eigen::MatrixXd &X, Eigen::MatrixXd *X_pinv) -{ +inline bool pseudoinverse(const Eigen::MatrixXd &X, Eigen::MatrixXd *X_pinv) { Eigen::MatrixXd copy = X.transpose() * (X * X.transpose()).inverse(); - if (isFucked(copy)) - { + if (isFucked(copy)) { return false; } *X_pinv = copy; @@ -62,17 +55,14 @@ inline bool pseudoinverse(const Eigen::MatrixXd &X, Eigen::MatrixXd *X_pinv) } // Return the 3-by-3 skew-symmetric matrix of the vector v. -inline Eigen::Matrix3d skew(const Eigen::Vector3d &v) -{ +inline Eigen::Matrix3d skew(const Eigen::Vector3d &v) { Eigen::Matrix3d S; - S << 0 , -v(2), v(1), - v(2), 0 , -v(0), - -v(1), v(0), 0; + S << 0, -v(2), v(1), v(2), 0, -v(0), -v(1), v(0), 0; return S; } -inline void arrayEigenToMsg(const Eigen::VectorXd &u, std_msgs::Float32MultiArray *msg) -{ +inline void arrayEigenToMsg(const Eigen::VectorXd &u, + std_msgs::Float32MultiArray *msg) { int r = u.size(); std::vector u_vec(r); for (int i = 0; i < r; ++i) @@ -82,18 +72,13 @@ inline void arrayEigenToMsg(const Eigen::VectorXd &u, std_msgs::Float32MultiArra // Saturate all elements of vector v to within [min, max]. // Return true if all elements already are within the range. -inline bool saturateVector(Eigen::VectorXd *v, double min, double max) -{ +inline bool saturateVector(Eigen::VectorXd *v, double min, double max) { bool vector_in_range = true; - for (int i = 0; i < v->size(); ++i) - { - if ((*v)(i) > max) - { + for (int i = 0; i < v->size(); ++i) { + if ((*v)(i) > max) { (*v)(i) = max; vector_in_range = false; - } - else if ((*v)(i) < min) - { + } else if ((*v)(i) < min) { (*v)(i) = min; vector_in_range = false; } @@ -101,4 +86,4 @@ inline bool saturateVector(Eigen::VectorXd *v, double min, double max) return vector_in_range; } -#endif // VORTEX_EIGEN_HELPER_H +#endif // VORTEX_EIGEN_HELPER_H diff --git a/motion/thruster_allocator/include/vortex_allocator/eigen_typedefs.h b/motion/thruster_allocator/include/vortex_allocator/eigen_typedefs.h index 6c88775cf..33e3804fb 100644 --- a/motion/thruster_allocator/include/vortex_allocator/eigen_typedefs.h +++ b/motion/thruster_allocator/include/vortex_allocator/eigen_typedefs.h @@ -3,11 +3,10 @@ #include -namespace Eigen -{ +namespace Eigen { typedef Eigen::Matrix Matrix6d; typedef Eigen::Matrix Vector6d; typedef Eigen::Matrix Vector8d; -} +} // namespace Eigen -#endif // VORTEX_EIGEN_TYPEDEFS_H +#endif // VORTEX_EIGEN_TYPEDEFS_H diff --git a/motion/thruster_allocator/include/vortex_allocator/pseudoinverse_allocator.h b/motion/thruster_allocator/include/vortex_allocator/pseudoinverse_allocator.h index 09ec041c4..d421a0063 100644 --- a/motion/thruster_allocator/include/vortex_allocator/pseudoinverse_allocator.h +++ b/motion/thruster_allocator/include/vortex_allocator/pseudoinverse_allocator.h @@ -7,8 +7,7 @@ #include -class PseudoinverseAllocator -{ +class PseudoinverseAllocator { public: explicit PseudoinverseAllocator(const Eigen::MatrixXd &T_pinv); Eigen::VectorXd compute(const Eigen::VectorXd &tau); @@ -17,4 +16,4 @@ class PseudoinverseAllocator Eigen::MatrixXd T_pinv; }; -#endif // VORTEX_ALLOCATOR_PSEUDOINVERSE_ALLOCATOR_H +#endif // VORTEX_ALLOCATOR_PSEUDOINVERSE_ALLOCATOR_H diff --git a/motion/thruster_allocator/src/allocator_node.cpp b/motion/thruster_allocator/src/allocator_node.cpp index f9c132b86..6b7fc8bd3 100644 --- a/motion/thruster_allocator/src/allocator_node.cpp +++ b/motion/thruster_allocator/src/allocator_node.cpp @@ -2,8 +2,7 @@ #include "vortex_allocator/allocator_ros.h" -int main(int argc, char **argv) -{ +int main(int argc, char **argv) { ros::init(argc, argv, "allocator"); ros::NodeHandle nh; Allocator allocator(nh); diff --git a/motion/thruster_allocator/src/allocator_ros.cpp b/motion/thruster_allocator/src/allocator_ros.cpp index 853e9e4e9..af93ad724 100644 --- a/motion/thruster_allocator/src/allocator_ros.cpp +++ b/motion/thruster_allocator/src/allocator_ros.cpp @@ -1,7 +1,6 @@ #include "vortex_allocator/allocator_ros.h" -Allocator::Allocator(ros::NodeHandle nh) : m_nh(nh) -{ +Allocator::Allocator(ros::NodeHandle nh) : m_nh(nh) { // parameters if (!m_nh.getParam("/propulsion/dofs/num", m_num_degrees_of_freedom)) ROS_FATAL("Failed to read parameter number of dofs."); @@ -11,39 +10,44 @@ Allocator::Allocator(ros::NodeHandle nh) : m_nh(nh) ROS_FATAL("Failed to read parameter which dofs."); // Read thrust config matrix - if (!getMatrixParam(m_nh, "/propulsion/thrusters/configuration_matrix", &thrust_configuration)) - { + if (!getMatrixParam(m_nh, "/propulsion/thrusters/configuration_matrix", + &thrust_configuration)) { ROS_FATAL("Failed to read parameter thrust config matrix. Killing node..."); ros::shutdown(); } // calculate pseudo inverse of thrust config matrix Eigen::MatrixXd thrust_configuration_pseudoinverse; - if (!pseudoinverse(thrust_configuration, &thrust_configuration_pseudoinverse)) - { - ROS_FATAL("Failed to compute pseudoinverse of thrust config matrix. Killing node..."); + if (!pseudoinverse(thrust_configuration, + &thrust_configuration_pseudoinverse)) { + ROS_FATAL("Failed to compute pseudoinverse of thrust config matrix. " + "Killing node..."); ros::shutdown(); } // publishers and subscribers - m_sub = m_nh.subscribe("/thrust/desired_forces", 1, &Allocator::callback, this); - m_pub = m_nh.advertise("/thrust/thruster_forces", 1); - - delivered_forces_pub = m_nh.advertise("tau_delivered", 1); - thruster_forces_sub = m_nh.subscribe("/thrust/delivered_forces", 1, &Allocator::thrusterForcesCb, this); - - m_pseudoinverse_allocator.reset(new PseudoinverseAllocator(thrust_configuration_pseudoinverse)); + m_sub = + m_nh.subscribe("/thrust/desired_forces", 1, &Allocator::callback, this); + m_pub = + m_nh.advertise("/thrust/thruster_forces", 1); + + delivered_forces_pub = + m_nh.advertise("tau_delivered", 1); + thruster_forces_sub = m_nh.subscribe("/thrust/delivered_forces", 1, + &Allocator::thrusterForcesCb, this); + + m_pseudoinverse_allocator.reset( + new PseudoinverseAllocator(thrust_configuration_pseudoinverse)); ROS_INFO("Initialized."); } -void Allocator::callback(const geometry_msgs::Wrench& msg_in) const -{ +void Allocator::callback(const geometry_msgs::Wrench &msg_in) const { const Eigen::VectorXd rov_forces = rovForcesMsgToEigen(msg_in); - Eigen::VectorXd thruster_forces = m_pseudoinverse_allocator->compute(rov_forces); + Eigen::VectorXd thruster_forces = + m_pseudoinverse_allocator->compute(rov_forces); - if (isFucked(thruster_forces)) - { + if (isFucked(thruster_forces)) { ROS_ERROR("Thruster forces vector invalid, ignoring."); return; } @@ -54,8 +58,8 @@ void Allocator::callback(const geometry_msgs::Wrench& msg_in) const m_pub.publish(msg_out); } -Eigen::VectorXd Allocator::rovForcesMsgToEigen(const geometry_msgs::Wrench& msg) const -{ +Eigen::VectorXd +Allocator::rovForcesMsgToEigen(const geometry_msgs::Wrench &msg) const { Eigen::VectorXd rov_forces(m_num_degrees_of_freedom); unsigned i = 0; if (m_active_degrees_of_freedom.at("surge")) @@ -71,22 +75,21 @@ Eigen::VectorXd Allocator::rovForcesMsgToEigen(const geometry_msgs::Wrench& msg) if (m_active_degrees_of_freedom.at("yaw")) rov_forces(i++) = msg.torque.z; - if (i != m_num_degrees_of_freedom) - { - ROS_WARN_STREAM("Invalid length of rov_forces vector. Is " << i << ", should be " << m_num_degrees_of_freedom - << ". Returning zero thrust vector."); + if (i != m_num_degrees_of_freedom) { + ROS_WARN_STREAM("Invalid length of rov_forces vector. Is " + << i << ", should be " << m_num_degrees_of_freedom + << ". Returning zero thrust vector."); return Eigen::VectorXd::Zero(m_num_degrees_of_freedom); } return rov_forces; } -void Allocator::thrusterForcesCb(const std_msgs::Float32MultiArray& thruster_forces_msg) -{ +void Allocator::thrusterForcesCb( + const std_msgs::Float32MultiArray &thruster_forces_msg) { // convert msg to eigen Eigen::Vector8d thruster_forces; - for (int i=0; i<8; i++) - { + for (int i = 0; i < 8; i++) { thruster_forces[i] = thruster_forces_msg.data[i]; } diff --git a/motion/thruster_allocator/src/pseudoinverse_allocator.cpp b/motion/thruster_allocator/src/pseudoinverse_allocator.cpp index 9c19ba153..4eec2d063 100644 --- a/motion/thruster_allocator/src/pseudoinverse_allocator.cpp +++ b/motion/thruster_allocator/src/pseudoinverse_allocator.cpp @@ -1,10 +1,9 @@ #include "vortex_allocator/pseudoinverse_allocator.h" PseudoinverseAllocator::PseudoinverseAllocator(const Eigen::MatrixXd &T_pinv) - : T_pinv(T_pinv) {} + : T_pinv(T_pinv) {} -Eigen::VectorXd PseudoinverseAllocator::compute(const Eigen::VectorXd &tau) -{ +Eigen::VectorXd PseudoinverseAllocator::compute(const Eigen::VectorXd &tau) { Eigen::VectorXd u = T_pinv * tau; return u; } diff --git a/motion/thruster_allocator/test/allocator_test.cpp b/motion/thruster_allocator/test/allocator_test.cpp index b54c44682..ad8deb60f 100644 --- a/motion/thruster_allocator/test/allocator_test.cpp +++ b/motion/thruster_allocator/test/allocator_test.cpp @@ -1,14 +1,12 @@ -#include "ros/ros.h" -#include #include "geometry_msgs/Wrench.h" +#include "ros/ros.h" #include "vortex_msgs/ThrusterForces.h" +#include #include -class AllocatorTest : public ::testing::Test -{ +class AllocatorTest : public ::testing::Test { public: - AllocatorTest() - { + AllocatorTest() { pub = nh.advertise("rov_forces", 10); sub = nh.subscribe("thruster_forces", 10, &AllocatorTest::Callback, this); message_received = false; @@ -19,66 +17,59 @@ class AllocatorTest : public ::testing::Test thrust.resize(num_thrusters); } - void SetUp() - { + void SetUp() { while (!IsNodeReady()) ros::spinOnce(); } - void Publish(double surge, double sway, double heave, double roll, double pitch, double yaw) - { + void Publish(double surge, double sway, double heave, double roll, + double pitch, double yaw) { geometry_msgs::Wrench msg; - msg.force.x = surge; - msg.force.y = sway; - msg.force.z = heave; + msg.force.x = surge; + msg.force.y = sway; + msg.force.z = heave; msg.torque.x = roll; msg.torque.y = pitch; msg.torque.z = yaw; pub.publish(msg); } - void ExpectThrustNear(double* arr) - { + void ExpectThrustNear(double *arr) { for (int i = 0; i < thrust.size(); ++i) EXPECT_NEAR(thrust[i], arr[i], MAX_ERROR); } - void WaitForMessage() - { + void WaitForMessage() { while (!message_received) ros::spinOnce(); } - int num_thrusters; + int num_thrusters; std::vector thrust; - const double MAX_ERROR = 1e-4; + const double MAX_ERROR = 1e-4; private: ros::NodeHandle nh; - ros::Publisher pub; + ros::Publisher pub; ros::Subscriber sub; bool message_received; - void Callback(const vortex_msgs::ThrusterForces& msg) - { + void Callback(const vortex_msgs::ThrusterForces &msg) { thrust = msg.thrust; message_received = true; } - bool IsNodeReady() - { + bool IsNodeReady() { return (pub.getNumSubscribers() > 0) && (sub.getNumPublishers() > 0); } }; -TEST_F(AllocatorTest, CheckResponsiveness) -{ +TEST_F(AllocatorTest, CheckResponsiveness) { Publish(0, 0, 0, 0, 0, 0); WaitForMessage(); } -TEST_F(AllocatorTest, ZeroInput) -{ +TEST_F(AllocatorTest, ZeroInput) { Publish(0, 0, 0, 0, 0, 0); WaitForMessage(); @@ -86,17 +77,16 @@ TEST_F(AllocatorTest, ZeroInput) ExpectThrustNear(thrust_expected); } -TEST_F(AllocatorTest, Forward) -{ +TEST_F(AllocatorTest, Forward) { Publish(1, 0, 0, 0, 0, 0); WaitForMessage(); - double thrust_expected[] = {0.35356, 0.35356, -0.35356, -0.35356, -0.20639, 0.20639}; + double thrust_expected[] = {0.35356, 0.35356, -0.35356, + -0.35356, -0.20639, 0.20639}; ExpectThrustNear(thrust_expected); } -TEST_F(AllocatorTest, Sideways) -{ +TEST_F(AllocatorTest, Sideways) { Publish(0, 1, 0, 0, 0, 0); WaitForMessage(); @@ -104,8 +94,7 @@ TEST_F(AllocatorTest, Sideways) ExpectThrustNear(thrust_expected); } -TEST_F(AllocatorTest, Downward) -{ +TEST_F(AllocatorTest, Downward) { Publish(0, 0, 1, 0, 0, 0); WaitForMessage(); @@ -113,8 +102,7 @@ TEST_F(AllocatorTest, Downward) ExpectThrustNear(thrust_expected); } -TEST_F(AllocatorTest, TiltUp) -{ +TEST_F(AllocatorTest, TiltUp) { Publish(0, 0, 0, 0, 1, 0); WaitForMessage(); @@ -122,8 +110,7 @@ TEST_F(AllocatorTest, TiltUp) ExpectThrustNear(thrust_expected); } -TEST_F(AllocatorTest, TurnRight) -{ +TEST_F(AllocatorTest, TurnRight) { Publish(0, 0, 0, 0, 0, 1); WaitForMessage(); @@ -131,8 +118,7 @@ TEST_F(AllocatorTest, TurnRight) ExpectThrustNear(thrust_expected); } -int main(int argc, char **argv) -{ +int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "allocator_test"); diff --git a/motion/velocity_controller/include/velocity_controller/velocity_controller.h b/motion/velocity_controller/include/velocity_controller/velocity_controller.h index 53b1c5872..40ffd342a 100644 --- a/motion/velocity_controller/include/velocity_controller/velocity_controller.h +++ b/motion/velocity_controller/include/velocity_controller/velocity_controller.h @@ -1,15 +1,15 @@ #ifndef VELOCITY_CONTROLLER_H #define VELOCITY_CONTROLLER_H +#include // for std::make_unique #include #include -#include // for std::make_unique -#include -#include -#include #include #include +#include +#include +#include #include #include @@ -20,20 +20,18 @@ #include "vortex_msgs/SetVelocity.h" // These typdefs are lacking from the default eigen namespace -namespace Eigen -{ +namespace Eigen { typedef Eigen::Matrix Matrix6d; typedef Eigen::Matrix Vector6d; -} // namespace Eigen +} // namespace Eigen /** - * @brief class of a velocity controller that uses six one dimensional PID controllers - * with feed-forward term and integral windup protection. The control law includes - * compensation for restoring forces. + * @brief class of a velocity controller that uses six one dimensional PID + * controllers with feed-forward term and integral windup protection. The + * control law includes compensation for restoring forces. * */ -class VelocityController -{ +class VelocityController { public: /** * @brief Construct a new Velocity Controller object @@ -41,67 +39,74 @@ class VelocityController * @param nh ros node handle */ VelocityController(ros::NodeHandle nh); - - /** + + /** * @brief spin function - * - * - */ + * + * + */ void spin(); private: /** - * @brief updates local copy of velocity as eigen vector and orientation as quaternion + * @brief updates local copy of velocity as eigen vector and orientation as + * quaternion * * @param odom_msg message with odometry data */ - void odometryCallback(const nav_msgs::Odometry& odom_msg); + void odometryCallback(const nav_msgs::Odometry &odom_msg); /** - *@brief Set desired velocity and activate or deactivate controller - * + *@brief Set desired velocity and activate or deactivate controller + * * @param SetVelocity msg with desired velocity and active bool */ - bool setVelocity(vortex_msgs::SetVelocityRequest& req, vortex_msgs::SetVelocityResponse& res); + bool setVelocity(vortex_msgs::SetVelocityRequest &req, + vortex_msgs::SetVelocityResponse &res); /** - * @brief publishes a thrust given by a desired velocity using a PID. Only non-zero desired velocities - * are controlled. + * @brief publishes a thrust given by a desired velocity using a PID. Only + * non-zero desired velocities are controlled. * * @param twist_msg message with desired velocity */ void publishControlForces(); /** - * @brief resets all six PIDs. Clears I and D term and sets setpoint to current position. + * @brief resets all six PIDs. Clears I and D term and sets setpoint to + * current position. * * @param request empty * @param response empty * @return true if reset did not crash */ - bool resetPidCallback(std_srvs::EmptyRequest& request, std_srvs::EmptyResponse& response); + bool resetPidCallback(std_srvs::EmptyRequest &request, + std_srvs::EmptyResponse &response); /** - * @brief Sets new P, I, D, F and windup terms for all six PIDs. Does a reset of PIDs. + * @brief Sets new P, I, D, F and windup terms for all six PIDs. Does a reset + * of PIDs. * * @param request new gains that PIDs should be set to * @param response empty * @return true if operation did not crash */ - bool setGainsCallback(vortex_msgs::SetPidGainsRequest& request, vortex_msgs::SetPidGainsResponse& response); + bool setGainsCallback(vortex_msgs::SetPidGainsRequest &request, + vortex_msgs::SetPidGainsResponse &response); /** - * @brief local wrapper around ros::getParam(). Shuts down node if param is not found. + * @brief local wrapper around ros::getParam(). Shuts down node if param is + * not found. * * @tparam T string, double, float, int bool or vectors of these * @param name name of the param on the ros network * @param variable variable the param will be saved in */ - template - void getParam(std::string name, T& variable); + template void getParam(std::string name, T &variable); /** - * @brief local wrapper around ros::getParam(). Sets variable to default value if param is not found. + * @brief local wrapper around ros::getParam(). Sets variable to default value + * if param is not found. * * @tparam T string, double, float, int bool or vectors of these * @param name name of the param on the ros network @@ -109,13 +114,11 @@ class VelocityController * @param default_value default value that variable will be set to */ template - void getParam(std::string name, T& variable, T& default_value); - - - + void getParam(std::string name, T &variable, T &default_value); /** - * @brief calculates resotring forces acting on drone caused by buoyancy and gravity + * @brief calculates resotring forces acting on drone caused by buoyancy and + * gravity * * @return Eigen::Vector6d vector containing restoring forces */ diff --git a/motion/velocity_controller/libs/MiniPID/MiniPID.cpp b/motion/velocity_controller/libs/MiniPID/MiniPID.cpp index 18cc51685..2d392731c 100644 --- a/motion/velocity_controller/libs/MiniPID/MiniPID.cpp +++ b/motion/velocity_controller/libs/MiniPID/MiniPID.cpp @@ -1,327 +1,342 @@ /** -* Small, easy to use PID implementation with advanced controller capability.
-* Minimal usage:
-* setPID(p,i,d);
-* ...looping code...{
-* output=getOutput(sensorvalue,target);
-* } -* -* @see http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-direction/improving-the-beginners-pid-introduction -*/ + * Small, easy to use PID implementation with advanced controller + * capability.
Minimal usage:
setPID(p,i,d);
+ * ...looping code...{
+ * output=getOutput(sensorvalue,target);
+ * } + * + * @see + * http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-direction/improving-the-beginners-pid-introduction + */ #include "MiniPID.h" //********************************** -//Constructor functions +// Constructor functions //********************************** -MiniPID::MiniPID(double p, double i, double d){ - init(); - P=p; I=i; D=d; +MiniPID::MiniPID(double p, double i, double d) { + init(); + P = p; + I = i; + D = d; } -MiniPID::MiniPID(double p, double i, double d, double f){ - init(); - P=p; I=i; D=d; F=f; +MiniPID::MiniPID(double p, double i, double d, double f) { + init(); + P = p; + I = i; + D = d; + F = f; } -void MiniPID::init(){ - P=0; - I=0; - D=0; - F=0; - - maxIOutput=0; - maxError=0; - errorSum=0; - maxOutput=0; - minOutput=0; - setpoint=0; - lastActual=0; - firstRun=true; - reversed=false; - outputRampRate=0; - lastOutput=0; - outputFilter=0; - setpointRange=0; +void MiniPID::init() { + P = 0; + I = 0; + D = 0; + F = 0; + + maxIOutput = 0; + maxError = 0; + errorSum = 0; + maxOutput = 0; + minOutput = 0; + setpoint = 0; + lastActual = 0; + firstRun = true; + reversed = false; + outputRampRate = 0; + lastOutput = 0; + outputFilter = 0; + setpointRange = 0; } //********************************** -//Configuration functions +// Configuration functions //********************************** /** * Configure the Proportional gain parameter.
- * this->responds quicly to changes in setpoint, and provides most of the initial driving force - * to make corrections.
- * Some systems can be used with only a P gain, and many can be operated with only PI.
- * For position based controllers, this->is the first parameter to tune, with I second.
- * For rate controlled systems, this->is often the second after F. + * this->responds quicly to changes in setpoint, and provides most of the + * initial driving force to make corrections.
Some systems can be used with + * only a P gain, and many can be operated with only PI.
For position based + * controllers, this->is the first parameter to tune, with I second.
For + * rate controlled systems, this->is often the second after F. * - * @param p Proportional gain. Affects output according to output+=P*(setpoint-current_value) + * @param p Proportional gain. Affects output according to + * output+=P*(setpoint-current_value) */ -void MiniPID::setP(double p){ - P=p; - checkSigns(); +void MiniPID::setP(double p) { + P = p; + checkSigns(); } /** * Changes the I parameter
- * this->is used for overcoming disturbances, and ensuring that the controller always gets to the control mode. - * Typically tuned second for "Position" based modes, and third for "Rate" or continuous based modes.
- * Affects output through output+=previous_errors*Igain ;previous_errors+=current_error - * + * this->is used for overcoming disturbances, and ensuring that the controller + * always gets to the control mode. Typically tuned second for "Position" based + * modes, and third for "Rate" or continuous based modes.
Affects output + * through output+=previous_errors*Igain ;previous_errors+=current_error + * * @see {@link #setMaxIOutput(double) setMaxIOutput} for how to restrict * * @param i New gain value for the Integral term */ -void MiniPID::setI(double i){ - if(I!=0){ - errorSum=errorSum*I/i; - } - if(maxIOutput!=0){ - maxError=maxIOutput/i; - } - I=i; - checkSigns(); - /* Implementation note: - * this->Scales the accumulated error to avoid output errors. - * As an example doubling the I term cuts the accumulated error in half, which results in the - * output change due to the I term constant during the transition. - * - */ -} - -void MiniPID::setD(double d){ - D=d; - checkSigns(); +void MiniPID::setI(double i) { + if (I != 0) { + errorSum = errorSum * I / i; + } + if (maxIOutput != 0) { + maxError = maxIOutput / i; + } + I = i; + checkSigns(); + /* Implementation note: + * this->Scales the accumulated error to avoid output errors. + * As an example doubling the I term cuts the accumulated error in half, which + * results in the output change due to the I term constant during the + * transition. + * + */ +} + +void MiniPID::setD(double d) { + D = d; + checkSigns(); } /**Configure the FeedForward parameter.
- * this->is excellent for Velocity, rate, and other continuous control modes where you can - * expect a rough output value based solely on the setpoint.
+ * this->is excellent for Velocity, rate, and other continuous control modes + * where you can expect a rough output value based solely on the setpoint.
* Should not be used in "position" based control modes. - * - * @param f Feed forward gain. Affects output according to output+=F*Setpoint; + * + * @param f Feed forward gain. Affects output according to + * output+=F*Setpoint; */ -void MiniPID::setF(double f){ - F=f; - checkSigns(); +void MiniPID::setF(double f) { + F = f; + checkSigns(); } -/** Create a new PID object. - * @param p Proportional gain. Large if large difference between setpoint and target. - * @param i Integral gain. Becomes large if setpoint cannot reach target quickly. - * @param d Derivative gain. Responds quickly to large changes in error. Small values prevents P and I terms from causing overshoot. +/** Create a new PID object. + * @param p Proportional gain. Large if large difference between setpoint and + * target. + * @param i Integral gain. Becomes large if setpoint cannot reach target + * quickly. + * @param d Derivative gain. Responds quickly to large changes in error. Small + * values prevents P and I terms from causing overshoot. */ -void MiniPID::setPID(double p, double i, double d){ - P=p;I=i;D=d; - checkSigns(); +void MiniPID::setPID(double p, double i, double d) { + P = p; + I = i; + D = d; + checkSigns(); } -void MiniPID::setPID(double p, double i, double d,double f){ - P=p;I=i;D=d;F=f; - checkSigns(); +void MiniPID::setPID(double p, double i, double d, double f) { + P = p; + I = i; + D = d; + F = f; + checkSigns(); } /**Set the maximum output value contributed by the I component of the system * this->can be used to prevent large windup issues and make tuning simpler * @param maximum. Units are the same as the expected output value */ -void MiniPID::setMaxIOutput(double maximum){ - /* Internally maxError and Izone are similar, but scaled for different purposes. - * The maxError is generated for simplifying math, since calculations against - * the max error are far more common than changing the I term or Izone. - */ - maxIOutput=maximum; - if(I!=0){ - maxError=maxIOutput/I; - } +void MiniPID::setMaxIOutput(double maximum) { + /* Internally maxError and Izone are similar, but scaled for different + * purposes. The maxError is generated for simplifying math, since + * calculations against the max error are far more common than changing the I + * term or Izone. + */ + maxIOutput = maximum; + if (I != 0) { + maxError = maxIOutput / I; + } } -/**Specify a maximum output. If a single parameter is specified, the minimum is +/**Specify a maximum output. If a single parameter is specified, the minimum is * set to (-maximum). - * @param output + * @param output */ -void MiniPID::setOutputLimits(double output){ setOutputLimits(-output,output);} +void MiniPID::setOutputLimits(double output) { + setOutputLimits(-output, output); +} /** * Specify a maximum output. * @param minimum possible output value * @param maximum possible output value */ -void MiniPID::setOutputLimits(double minimum,double maximum){ - if(maximum(maximum-minimum) ){ - setMaxIOutput(maximum-minimum); - } +void MiniPID::setOutputLimits(double minimum, double maximum) { + if (maximum < minimum) + return; + maxOutput = maximum; + minOutput = minimum; + + // Ensure the bounds of the I term are within the bounds of the allowable + // output swing + if (maxIOutput == 0 || maxIOutput > (maximum - minimum)) { + setMaxIOutput(maximum - minimum); + } } /** Set the operating direction of the PID controller * @param reversed Set true to reverse PID output */ -void MiniPID::setDirection(bool reversed){ - this->reversed=reversed; -} +void MiniPID::setDirection(bool reversed) { this->reversed = reversed; } //********************************** -//Primary operating functions +// Primary operating functions //********************************** /**Set the target for the PID calculations * @param setpoint */ -void MiniPID::setSetpoint(double setpoint){ - this->setpoint=setpoint; -} - -/** Calculate the PID value needed to hit the target setpoint. -* Automatically re-calculates the output at each call. -* @param actual The monitored value -* @param target The target value -* @return calculated output value for driving the actual to the target -*/ -double MiniPID::getOutput(double actual, double setpoint){ - double output; - double Poutput; - double Ioutput; - double Doutput; - double Foutput; - - this->setpoint=setpoint; - - //Ramp the setpoint used for calculations if user has opted to do so - if(setpointRange!=0){ - setpoint=clamp(setpoint,actual-setpointRange,actual+setpointRange); - } - - //Do the simple parts of the calculations - double error=setpoint-actual; - - //Calculate F output. Notice, this->depends only on the setpoint, and not the error. - Foutput=F*setpoint; - - //Calculate P term - Poutput=P*error; - - //If this->is our first time running this-> we don't actually _have_ a previous input or output. - //For sensor, sanely assume it was exactly where it is now. - //For last output, we can assume it's the current time-independent outputs. - if(firstRun){ - lastActual=actual; - lastOutput=Poutput+Foutput; - firstRun=false; - } - - - //Calculate D Term - //Note, this->is negative. this->actually "slows" the system if it's doing - //the correct thing, and small values helps prevent output spikes and overshoot - - Doutput= -D*(actual-lastActual); - lastActual=actual; - - - - //The Iterm is more complex. There's several things to factor in to make it easier to deal with. - // 1. maxIoutput restricts the amount of output contributed by the Iterm. - // 2. prevent windup by not increasing errorSum if we're already running against our max Ioutput - // 3. prevent windup by not increasing errorSum if output is output=maxOutput - Ioutput=I*errorSum; - if(maxIOutput!=0){ - Ioutput=clamp(Ioutput,-maxIOutput,maxIOutput); - } - - //And, finally, we can just add the terms up - output=Foutput + Poutput + Ioutput + Doutput; - - //Figure out what we're doing with the error. - if(minOutput!=maxOutput && !bounded(output, minOutput,maxOutput) ){ - errorSum=error; - // reset the error sum to a sane level - // Setting to current error ensures a smooth transition when the P term - // decreases enough for the I term to start acting upon the controller - // From that point the I term will build up as would be expected - } - else if(outputRampRate!=0 && !bounded(output, lastOutput-outputRampRate,lastOutput+outputRampRate) ){ - errorSum=error; - } - else if(maxIOutput!=0){ - errorSum=clamp(errorSum+error,-maxError,maxError); - // In addition to output limiting directly, we also want to prevent I term - // buildup, so restrict the error directly - } - else{ - errorSum+=error; - } - - //Restrict output to our specified output and ramp limits - if(outputRampRate!=0){ - output=clamp(output, lastOutput-outputRampRate,lastOutput+outputRampRate); - } - if(minOutput!=maxOutput){ - output=clamp(output, minOutput,maxOutput); - } - if(outputFilter!=0){ - output=lastOutput*outputFilter+output*(1-outputFilter); - } - - lastOutput=output; - return output; +void MiniPID::setSetpoint(double setpoint) { this->setpoint = setpoint; } + +/** Calculate the PID value needed to hit the target setpoint. + * Automatically re-calculates the output at each call. + * @param actual The monitored value + * @param target The target value + * @return calculated output value for driving the actual to the target + */ +double MiniPID::getOutput(double actual, double setpoint) { + double output; + double Poutput; + double Ioutput; + double Doutput; + double Foutput; + + this->setpoint = setpoint; + + // Ramp the setpoint used for calculations if user has opted to do so + if (setpointRange != 0) { + setpoint = clamp(setpoint, actual - setpointRange, actual + setpointRange); + } + + // Do the simple parts of the calculations + double error = setpoint - actual; + + // Calculate F output. Notice, this->depends only on the setpoint, and not the + // error. + Foutput = F * setpoint; + + // Calculate P term + Poutput = P * error; + + // If this->is our first time running this-> we don't actually _have_ a + // previous input or output. For sensor, sanely assume it was exactly where it + // is now. For last output, we can assume it's the current time-independent + // outputs. + if (firstRun) { + lastActual = actual; + lastOutput = Poutput + Foutput; + firstRun = false; + } + + // Calculate D Term + // Note, this->is negative. this->actually "slows" the system if it's doing + // the correct thing, and small values helps prevent output spikes and + // overshoot + + Doutput = -D * (actual - lastActual); + lastActual = actual; + + // The Iterm is more complex. There's several things to factor in to make it + // easier to deal with. + // 1. maxIoutput restricts the amount of output contributed by the Iterm. + // 2. prevent windup by not increasing errorSum if we're already running + // against our max Ioutput + // 3. prevent windup by not increasing errorSum if output is output=maxOutput + Ioutput = I * errorSum; + if (maxIOutput != 0) { + Ioutput = clamp(Ioutput, -maxIOutput, maxIOutput); + } + + // And, finally, we can just add the terms up + output = Foutput + Poutput + Ioutput + Doutput; + + // Figure out what we're doing with the error. + if (minOutput != maxOutput && !bounded(output, minOutput, maxOutput)) { + errorSum = error; + // reset the error sum to a sane level + // Setting to current error ensures a smooth transition when the P term + // decreases enough for the I term to start acting upon the controller + // From that point the I term will build up as would be expected + } else if (outputRampRate != 0 && + !bounded(output, lastOutput - outputRampRate, + lastOutput + outputRampRate)) { + errorSum = error; + } else if (maxIOutput != 0) { + errorSum = clamp(errorSum + error, -maxError, maxError); + // In addition to output limiting directly, we also want to prevent I term + // buildup, so restrict the error directly + } else { + errorSum += error; + } + + // Restrict output to our specified output and ramp limits + if (outputRampRate != 0) { + output = + clamp(output, lastOutput - outputRampRate, lastOutput + outputRampRate); + } + if (minOutput != maxOutput) { + output = clamp(output, minOutput, maxOutput); + } + if (outputFilter != 0) { + output = lastOutput * outputFilter + output * (1 - outputFilter); + } + + lastOutput = output; + return output; } /** * Calculates the PID value using the last provided setpoint and actual valuess - * @return calculated output value for driving the actual to the target + * @return calculated output value for driving the actual to the target */ -double MiniPID::getOutput(){ - return getOutput(lastActual,setpoint); -} +double MiniPID::getOutput() { return getOutput(lastActual, setpoint); } /** - * + * * @param actual - * @return calculated output value for driving the actual to the target + * @return calculated output value for driving the actual to the target */ -double MiniPID::getOutput(double actual){ - return getOutput(actual,setpoint); -} - +double MiniPID::getOutput(double actual) { return getOutput(actual, setpoint); } + /** - * Resets the controller. this->erases the I term buildup, and removes D gain on the next loop. + * Resets the controller. this->erases the I term buildup, and removes D gain on + * the next loop. */ -void MiniPID::reset(){ - firstRun=true; - errorSum=0; +void MiniPID::reset() { + firstRun = true; + errorSum = 0; } -/**Set the maximum rate the output can increase per cycle. +/**Set the maximum rate the output can increase per cycle. * @param rate */ -void MiniPID::setOutputRampRate(double rate){ - outputRampRate=rate; -} +void MiniPID::setOutputRampRate(double rate) { outputRampRate = rate; } /** Set a limit on how far the setpoint can be from the current position - *
Can simplify tuning by helping tuning over a small range applies to a much larger range. - *
this->limits the reactivity of P term, and restricts impact of large D term - * during large setpoint adjustments. Increases lag and I term if range is too small. + *
Can simplify tuning by helping tuning over a small range applies to a + * much larger range.
this->limits the reactivity of P term, and restricts + * impact of large D term during large setpoint adjustments. Increases lag and I + * term if range is too small. * @param range */ -void MiniPID::setSetpointRange(double range){ - setpointRange=range; -} +void MiniPID::setSetpointRange(double range) { setpointRange = range; } /**Set a filter on the output to reduce sharp oscillations.
- * 0.1 is likely a sane starting value. Larger values P and D oscillations, but force larger I values. - * Uses an exponential rolling sum filter, according to a simple
- * 
output*(1-strength)*sum(0..n){output*strength^n}
- * @param output valid between [0..1), meaning [current output only.. historical output only) + * 0.1 is likely a sane starting value. Larger values P and D oscillations, but + * force larger I values. Uses an exponential rolling sum filter, according to a + * simple 
output*(1-strength)*sum(0..n){output*strength^n}
+ * @param output valid between [0..1), meaning [current output only.. historical + * output only) */ -void MiniPID::setOutputFilter(double strength){ - if(strength==0 || bounded(strength,0,1)){ - outputFilter=strength; - } +void MiniPID::setOutputFilter(double strength) { + if (strength == 0 || bounded(strength, 0, 1)) { + outputFilter = strength; + } } //************************************** @@ -333,12 +348,16 @@ void MiniPID::setOutputFilter(double strength){ * @param value input value * @param min maximum returned value * @param max minimum value in range - * @return Value if it's within provided range, min or max otherwise + * @return Value if it's within provided range, min or max otherwise */ -double MiniPID::clamp(double value, double min, double max){ - if(value > max){ return max;} - if(value < min){ return min;} - return value; +double MiniPID::clamp(double value, double min, double max) { + if (value > max) { + return max; + } + if (value < min) { + return min; + } + return value; } /** @@ -348,25 +367,32 @@ double MiniPID::clamp(double value, double min, double max){ * @param max Maximum value of range * @return */ -bool MiniPID::bounded(double value, double min, double max){ - return (min0) P*=-1; - if(I>0) I*=-1; - if(D>0) D*=-1; - if(F>0) F*=-1; - } - else{ //all values should be above zero - if(P<0) P*=-1; - if(I<0) I*=-1; - if(D<0) D*=-1; - if(F<0) F*=-1; - } +void MiniPID::checkSigns() { + if (reversed) { // all values should be below zero + if (P > 0) + P *= -1; + if (I > 0) + I *= -1; + if (D > 0) + D *= -1; + if (F > 0) + F *= -1; + } else { // all values should be above zero + if (P < 0) + P *= -1; + if (I < 0) + I *= -1; + if (D < 0) + D *= -1; + if (F < 0) + F *= -1; + } } diff --git a/motion/velocity_controller/libs/MiniPID/MiniPID.h b/motion/velocity_controller/libs/MiniPID/MiniPID.h index 55ed3abbf..b6298a26f 100644 --- a/motion/velocity_controller/libs/MiniPID/MiniPID.h +++ b/motion/velocity_controller/libs/MiniPID/MiniPID.h @@ -1,58 +1,58 @@ #ifndef MINIPID_H #define MINIPID_H -class MiniPID{ +class MiniPID { public: - MiniPID(double, double, double); - MiniPID(double, double, double, double); - void setP(double); - void setI(double); - void setD(double); - void setF(double); - void setPID(double, double, double); - void setPID(double, double, double, double); - void setMaxIOutput(double); - void setOutputLimits(double); - void setOutputLimits(double,double); - void setDirection(bool); - void setSetpoint(double); - void reset(); - void setOutputRampRate(double); - void setSetpointRange(double); - void setOutputFilter(double); - double getOutput(); - double getOutput(double); - double getOutput(double, double); + MiniPID(double, double, double); + MiniPID(double, double, double, double); + void setP(double); + void setI(double); + void setD(double); + void setF(double); + void setPID(double, double, double); + void setPID(double, double, double, double); + void setMaxIOutput(double); + void setOutputLimits(double); + void setOutputLimits(double, double); + void setDirection(bool); + void setSetpoint(double); + void reset(); + void setOutputRampRate(double); + void setSetpointRange(double); + void setOutputFilter(double); + double getOutput(); + double getOutput(double); + double getOutput(double, double); private: - double clamp(double, double, double); - bool bounded(double, double, double); - void checkSigns(); - void init(); - double P; - double I; - double D; - double F; + double clamp(double, double, double); + bool bounded(double, double, double); + void checkSigns(); + void init(); + double P; + double I; + double D; + double F; - double maxIOutput; - double maxError; - double errorSum; + double maxIOutput; + double maxError; + double errorSum; - double maxOutput; - double minOutput; + double maxOutput; + double minOutput; - double setpoint; + double setpoint; - double lastActual; + double lastActual; - bool firstRun; - bool reversed; + bool firstRun; + bool reversed; - double outputRampRate; - double lastOutput; + double outputRampRate; + double lastOutput; - double outputFilter; + double outputFilter; - double setpointRange; + double setpointRange; }; #endif diff --git a/motion/velocity_controller/scripts/velocity_config_server.py b/motion/velocity_controller/scripts/velocity_config_server.py index e01673579..990a6ec36 100755 --- a/motion/velocity_controller/scripts/velocity_config_server.py +++ b/motion/velocity_controller/scripts/velocity_config_server.py @@ -6,18 +6,21 @@ from velocity_controller.cfg import vel_controllerConfig from geometry_msgs.msg import Twist + def callback(config, level): - - velocity_server = rospy.get_param("/controllers/velocity_controller/desired_velocity_topic") + + velocity_server = rospy.get_param( + "/controllers/velocity_controller/desired_velocity_topic" + ) rospy.wait_for_service(velocity_server) - set_velocity = rospy.ServiceProxy(velocity_server,SetVelocity) + set_velocity = rospy.ServiceProxy(velocity_server, SetVelocity) twist = Twist() - twist.linear.x = config.x_vel - twist.linear.y = config.y_vel - twist.linear.z = config.z_vel + twist.linear.x = config.x_vel + twist.linear.y = config.y_vel + twist.linear.z = config.z_vel twist.angular.x = config.roll_vel twist.angular.y = config.pitch_vel @@ -26,12 +29,13 @@ def callback(config, level): msg = SetVelocityRequest() msg.desired_velocity = twist msg.active = config.active - + set_velocity(msg) return config + if __name__ == "__main__": - rospy.init_node('velocity_config_server') + rospy.init_node("velocity_config_server") srv = Server(vel_controllerConfig, callback) rospy.spin() diff --git a/motion/velocity_controller/src/velocity_controller.cpp b/motion/velocity_controller/src/velocity_controller.cpp index f4d45df02..85b1fb928 100644 --- a/motion/velocity_controller/src/velocity_controller.cpp +++ b/motion/velocity_controller/src/velocity_controller.cpp @@ -1,14 +1,15 @@ #include "velocity_controller/velocity_controller.h" -VelocityController::VelocityController(ros::NodeHandle nh) : nh(nh) -{ +VelocityController::VelocityController(ros::NodeHandle nh) : nh(nh) { // get params std::string DEFAULT_ODOM_TOPIC = "/odometry/filtered"; std::string DEFAULT_THRUST_TOPIC = "/thrust/desired_forces"; std::string DEFAULT_VELOCITY_TOPIC = "/controllers/velocity/desired_velocity"; - getParam("/controllers/velocity_controller/odometry_topic", odometry_topic, DEFAULT_ODOM_TOPIC); + getParam("/controllers/velocity_controller/odometry_topic", odometry_topic, + DEFAULT_ODOM_TOPIC); getParam("/thrust/thrust_topic", thrust_topic, DEFAULT_THRUST_TOPIC); - getParam("/controllers/velocity_controller/desired_velocity_topic", desired_velocity_topic, DEFAULT_VELOCITY_TOPIC); + getParam("/controllers/velocity_controller/desired_velocity_topic", + desired_velocity_topic, DEFAULT_VELOCITY_TOPIC); getParam("/controllers/velocity_controller/rate", rate); std::vector CB; @@ -17,7 +18,8 @@ VelocityController::VelocityController(ros::NodeHandle nh) : nh(nh) getParam("/physical/buoyancy", drone_buoyancy); getParam("/physical/center_of_buoyancy", CB); getParam("/physical/center_of_mass", CG); - center_of_buoyancy = Eigen::Vector3d(CB[0], CB[1], CB[2]) / 1000; // convert from mm to m + center_of_buoyancy = + Eigen::Vector3d(CB[0], CB[1], CB[2]) / 1000; // convert from mm to m center_of_gravity = Eigen::Vector3d(CG[0], CG[1], CG[2]) / 1000; std::vector P_gains; @@ -33,21 +35,24 @@ VelocityController::VelocityController(ros::NodeHandle nh) : nh(nh) getParam("/controllers/velocity_controller/I_gains", I_gains); getParam("/controllers/velocity_controller/D_gains", D_gains); getParam("/controllers/velocity_controller/F_gains", F_gains); - getParam("/controllers/velocity_controller/integral_windup_limit", integral_windup_limit); + getParam("/controllers/velocity_controller/integral_windup_limit", + integral_windup_limit); getParam("/controllers/velocity_controller/setpoint_range", setpoint_range); - getParam("/controllers/velocity_controller/max_output_ramp_rate", max_output_ramp_rate); + getParam("/controllers/velocity_controller/max_output_ramp_rate", + max_output_ramp_rate); // initialize PIDs - for (int i = 0; i < 6; i++) - { - pids.push_back(std::make_unique(P_gains[i], I_gains[i], D_gains[i], F_gains[i])); + for (int i = 0; i < 6; i++) { + pids.push_back(std::make_unique(P_gains[i], I_gains[i], D_gains[i], + F_gains[i])); pids[i]->setMaxIOutput(integral_windup_limit); pids[i]->setSetpointRange(setpoint_range); pids[i]->setOutputRampRate(max_output_ramp_rate); - ROS_DEBUG_STREAM("pid" << i << " initialized with: " << P_gains[i] << " " << I_gains[i] << " " << D_gains[i] << " " + ROS_DEBUG_STREAM("pid" << i << " initialized with: " << P_gains[i] << " " + << I_gains[i] << " " << D_gains[i] << " " << F_gains[i]); - ROS_DEBUG_STREAM("pid_" << i << " address: " << &pids[i]); + ROS_DEBUG_STREAM("pid_" << i << " address: " << &pids[i]); } ROS_DEBUG_STREAM("integral_windup_limit: " << integral_windup_limit); ROS_DEBUG_STREAM("setpoint_range: " << setpoint_range); @@ -56,56 +61,54 @@ VelocityController::VelocityController(ros::NodeHandle nh) : nh(nh) // create subscribers and publsihers odom_recieved = false; thrust_pub = nh.advertise(thrust_topic, 1); - odom_sub = nh.subscribe(odometry_topic, 1, &VelocityController::odometryCallback, this); + odom_sub = nh.subscribe(odometry_topic, 1, + &VelocityController::odometryCallback, this); // create services - set_velocity_service = nh.advertiseService(desired_velocity_topic, &VelocityController::setVelocity, this); - reset_service = nh.advertiseService("reset_pid", &VelocityController::resetPidCallback, this); - set_gains_service = - nh.advertiseService("set_gains", &VelocityController::setGainsCallback, this); + set_velocity_service = nh.advertiseService( + desired_velocity_topic, &VelocityController::setVelocity, this); + reset_service = nh.advertiseService( + "reset_pid", &VelocityController::resetPidCallback, this); + set_gains_service = nh.advertiseService( + "set_gains", &VelocityController::setGainsCallback, this); // wait for first odometry message ROS_INFO("Waiting for odometry message.."); - } -void VelocityController::odometryCallback(const nav_msgs::Odometry& odom_msg) -{ - - if (!odom_recieved) - { +void VelocityController::odometryCallback(const nav_msgs::Odometry &odom_msg) { + + if (!odom_recieved) { odom_recieved = true; ROS_INFO("Odometry message recieved"); ROS_INFO("Velocity controller setup complete"); } tf::twistMsgToEigen(odom_msg.twist.twist, velocity); - orientation = Eigen::Quaterniond(odom_msg.pose.pose.orientation.w, odom_msg.pose.pose.orientation.x, - odom_msg.pose.pose.orientation.y, odom_msg.pose.pose.orientation.z); + orientation = Eigen::Quaterniond( + odom_msg.pose.pose.orientation.w, odom_msg.pose.pose.orientation.x, + odom_msg.pose.pose.orientation.y, odom_msg.pose.pose.orientation.z); } -bool VelocityController::setVelocity(vortex_msgs::SetVelocityRequest& req, vortex_msgs::SetVelocityResponse& res) -{ +bool VelocityController::setVelocity(vortex_msgs::SetVelocityRequest &req, + vortex_msgs::SetVelocityResponse &res) { controller_active = req.active; - if (!controller_active){ - desired_velocity << 0,0,0,0,0,0; - } else tf::twistMsgToEigen(req.desired_velocity, desired_velocity); + if (!controller_active) { + desired_velocity << 0, 0, 0, 0, 0, 0; + } else + tf::twistMsgToEigen(req.desired_velocity, desired_velocity); // transform desired velocity to eigen publishControlForces(); return true; } -void VelocityController::publishControlForces() -{ - if (!odom_recieved) - { - ROS_ERROR("Controller cannot perform since no odometry has been recieved. Ignoring callback."); - } - else - { - // calculate tau +void VelocityController::publishControlForces() { + if (!odom_recieved) { + ROS_ERROR("Controller cannot perform since no odometry has been recieved. " + "Ignoring callback."); + } else { + // calculate tau Eigen::Vector6d tau; - for (int i = 0; i < 6; i++) - { + for (int i = 0; i < 6; i++) { if (desired_velocity[i] != 0) tau[i] = pids[i]->getOutput(velocity[i], desired_velocity[i]); else @@ -119,8 +122,8 @@ void VelocityController::publishControlForces() } } -bool VelocityController::resetPidCallback(std_srvs::EmptyRequest& request, std_srvs::EmptyResponse& response) -{ +bool VelocityController::resetPidCallback(std_srvs::EmptyRequest &request, + std_srvs::EmptyResponse &response) { for (int i = 0; i < 6; i++) pids[i]->reset(); @@ -128,11 +131,10 @@ bool VelocityController::resetPidCallback(std_srvs::EmptyRequest& request, std_s return true; } -bool VelocityController::setGainsCallback(vortex_msgs::SetPidGainsRequest& request, - vortex_msgs::SetPidGainsResponse& response) -{ - for (int i = 0; i < 6; i++) - { +bool VelocityController::setGainsCallback( + vortex_msgs::SetPidGainsRequest &request, + vortex_msgs::SetPidGainsResponse &response) { + for (int i = 0; i < 6; i++) { pids[i]->setP(request.P_gains[i]); pids[i]->setI(request.I_gains[i]); pids[i]->setD(request.D_gains[i]); @@ -146,43 +148,40 @@ bool VelocityController::setGainsCallback(vortex_msgs::SetPidGainsRequest& reque return true; } -Eigen::Vector6d VelocityController::restoringForces() -{ +Eigen::Vector6d VelocityController::restoringForces() { // calculates restoring forces in ENU (not NED) Eigen::Matrix3d R = orientation.toRotationMatrix(); Eigen::Vector3d f_G = R.transpose() * Eigen::Vector3d(0, 0, -drone_weight); Eigen::Vector3d f_B = R.transpose() * Eigen::Vector3d(0, 0, drone_buoyancy); - return (Eigen::Vector6d() << f_G + f_B, center_of_gravity.cross(f_G) + center_of_buoyancy.cross(f_B)).finished(); + return (Eigen::Vector6d() << f_G + f_B, + center_of_gravity.cross(f_G) + center_of_buoyancy.cross(f_B)) + .finished(); } template -void VelocityController::getParam(std::string name, T& variable) -{ - if (!nh.getParam(name, variable)) - { +void VelocityController::getParam(std::string name, T &variable) { + if (!nh.getParam(name, variable)) { ROS_FATAL_STREAM("Missing parameter " << name << ". Shutting down node.."); ros::shutdown(); } } template -void VelocityController::getParam(std::string name, T& variable, T& default_value) -{ +void VelocityController::getParam(std::string name, T &variable, + T &default_value) { if (!nh.getParam(name, variable)) variable = default_value; ROS_DEBUG_STREAM(name << ": " << variable); } -void VelocityController::spin() -{ +void VelocityController::spin() { ros::Rate ros_rate(rate); - while (ros::ok()) - { + while (ros::ok()) { if (controller_active) { publishControlForces(); } ros::spinOnce(); - ros_rate.sleep(); + ros_rate.sleep(); } } diff --git a/motion/velocity_controller/src/velocity_controller_node.cpp b/motion/velocity_controller/src/velocity_controller_node.cpp index 0c850336c..a1361af55 100644 --- a/motion/velocity_controller/src/velocity_controller_node.cpp +++ b/motion/velocity_controller/src/velocity_controller_node.cpp @@ -1,16 +1,14 @@ #include "velocity_controller/velocity_controller.h" - -int main(int argc, char** argv) -{ - const bool DEBUG_MODE = false; // debug logs are printed to console when true +int main(int argc, char **argv) { + const bool DEBUG_MODE = false; // debug logs are printed to console when true ros::init(argc, argv, "velocity_controller"); ros::NodeHandle nh; - if (DEBUG_MODE) - { - ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug); + if (DEBUG_MODE) { + ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, + ros::console::levels::Debug); ros::console::notifyLoggerLevelsChanged(); } diff --git a/motion/vtf_guidance_and_control/scripts/auv_model.py b/motion/vtf_guidance_and_control/scripts/auv_model.py index b72d33208..c92a74ff2 100755 --- a/motion/vtf_guidance_and_control/scripts/auv_model.py +++ b/motion/vtf_guidance_and_control/scripts/auv_model.py @@ -5,6 +5,7 @@ import numpy as np from functions import R_from_eul + class AUVModel: def __init__(self, m, r_g, r_b, inertia, volume, M_A, D, rho=997.0, g=9.81): self.m = m @@ -21,8 +22,8 @@ def __init__(self, m, r_g, r_b, inertia, volume, M_A, D, rho=997.0, g=9.81): def get_gvect(self, eul): gvect = np.zeros(6) - W = self.m*self.g - B = self.rho*self.g*self.volume + W = self.m * self.g + B = self.rho * self.g * self.volume R = R_from_eul(eul) f_g = np.dot(R.T, [0, 0, W]) f_b = np.dot(R.T, [0, 0, -B]) @@ -30,13 +31,14 @@ def get_gvect(self, eul): gvect[3:] = np.cross(self.r_g, f_g) + np.cross(self.r_b, f_b) return -gvect + def get_M_RB(m, r_g, inertia): - M_11 = m*np.eye(3) - M_12 = -m*skew(r_g) - M_21 = m*skew(r_g) - M_22 = np.array(inertia) - m*skew(r_g)**2 + M_11 = m * np.eye(3) + M_12 = -m * skew(r_g) + M_21 = m * skew(r_g) + M_22 = np.array(inertia) - m * skew(r_g) ** 2 return np.block([[M_11, M_12], [M_21, M_22]]) + def skew(v): return np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]]) - diff --git a/motion/vtf_guidance_and_control/scripts/auv_simulator.py b/motion/vtf_guidance_and_control/scripts/auv_simulator.py index 48c199836..48db9488b 100755 --- a/motion/vtf_guidance_and_control/scripts/auv_simulator.py +++ b/motion/vtf_guidance_and_control/scripts/auv_simulator.py @@ -8,8 +8,15 @@ from control_system import DPControlSystem from functions import euler2, J_from_eul, skew -class AUVSimulator(): - def __init__(self, AUVModel, ControlAllocationSystem, DPControlSystem, absolute_relative_velocity_limit): + +class AUVSimulator: + def __init__( + self, + AUVModel, + ControlAllocationSystem, + DPControlSystem, + absolute_relative_velocity_limit, + ): self.control_allocation_system = ControlAllocationSystem self.auv_model = AUVModel self.dp_control_system = DPControlSystem @@ -23,7 +30,9 @@ def __init__(self, AUVModel, ControlAllocationSystem, DPControlSystem, absolute_ self.t = 0 self.online = False - def set_initial_conditions(self, eta, nu, t, dot_nu=[0,0,0,0,0,0], tau=[0,0,0,0,0,0]): + def set_initial_conditions( + self, eta, nu, t, dot_nu=[0, 0, 0, 0, 0, 0], tau=[0, 0, 0, 0, 0, 0] + ): eta[3] = 0 eta[4] = 0 nu[3] = 0 @@ -35,15 +44,24 @@ def set_initial_conditions(self, eta, nu, t, dot_nu=[0,0,0,0,0,0], tau=[0,0,0,0, self.t = t self.online = True - def generate_trajectory_for_dp(self, t, N, h, eta_ref, nu_ref=[0,0,0,0,0,0], dot_nu_ref=[0,0,0,0,0,0], dot_eta_c=[0,0,0,0,0,0]): + def generate_trajectory_for_dp( + self, + t, + N, + h, + eta_ref, + nu_ref=[0, 0, 0, 0, 0, 0], + dot_nu_ref=[0, 0, 0, 0, 0, 0], + dot_eta_c=[0, 0, 0, 0, 0, 0], + ): # Initialize datasaving eta_d = np.zeros((int(N), 6)) nu_d = np.zeros((int(N), 6)) dot_nu_d = np.zeros((int(N), 6)) # Simulate the AUV - for i, t in enumerate(list(np.linspace(t, t+h*N, N))): - + for i, t in enumerate(list(np.linspace(t, t + h * N, N))): + J = J_from_eul(self.eta[3:]) nu_c = np.dot(J.T, dot_eta_c) dot_v_c = np.dot(-skew([0, 0, self.nu[5]]), nu_c[:3]) @@ -51,36 +69,46 @@ def generate_trajectory_for_dp(self, t, N, h, eta_ref, nu_ref=[0,0,0,0,0,0], dot nu_r = self.nu - nu_c # Simulator control law - tau_unb = self.dp_control_system.pd_regulate(self.eta, self.nu, eta_ref, nu_ref, dot_nu_ref, dot_eta_c) - tau_b = self.control_allocation_system.program_feasible_control_forces(tau_unb) - tau_b[3] = 0 # Ensuring no actuation in roll or pitch + tau_unb = self.dp_control_system.pd_regulate( + self.eta, self.nu, eta_ref, nu_ref, dot_nu_ref, dot_eta_c + ) + tau_b = self.control_allocation_system.program_feasible_control_forces( + tau_unb + ) + tau_b[3] = 0 # Ensuring no actuation in roll or pitch tau_b[4] = 0 # Simulate the actuator dynamics - dot_tau = (tau_b - self.tau)/(self.control_allocation_system.rotor_time_constant*2) + dot_tau = (tau_b - self.tau) / ( + self.control_allocation_system.rotor_time_constant * 2 + ) self.tau = euler2(dot_tau, self.tau, h) # Simulate the AUV dynamics - self.dot_nu = dot_nu_c + np.dot(np.linalg.inv(self.auv_model.M), self.tau - np.dot(self.auv_model.D, nu_r) - self.auv_model.gvect) + self.dot_nu = dot_nu_c + np.dot( + np.linalg.inv(self.auv_model.M), + self.tau - np.dot(self.auv_model.D, nu_r) - self.auv_model.gvect, + ) self.nu = euler2(self.dot_nu, self.nu, h) # Obey relative linear velocity limits - abs_rel_vel = np.linalg.norm(np.array(self.nu[:3])-np.array(nu_c[:3])) + abs_rel_vel = np.linalg.norm(np.array(self.nu[:3]) - np.array(nu_c[:3])) if abs_rel_vel > self.absolute_relative_velocity_limit: - gain = abs_rel_vel/self.absolute_relative_velocity_limit + gain = abs_rel_vel / self.absolute_relative_velocity_limit for i, v in enumerate(self.nu[:3]): - self.nu[i] = v/gain + self.nu[i] = v / gain dot_eta = np.dot(J, self.nu) self.eta = euler2(dot_eta, self.eta, h) eta_d[i], nu_d[i], dot_nu_d[i] = self.eta, self.nu, self.dot_nu - #u_b = np.dot(self.control_allocation_system.pseudo_inv_input_matrix, tau_b) # Remove later + # u_b = np.dot(self.control_allocation_system.pseudo_inv_input_matrix, tau_b) # Remove later return eta_d, nu_d, dot_nu_d + def quadprog_solve_qp(P, q, G=None, h=None, A=None, b=None): - qp_G = .5 * (P + P.T) # make sure P is symmetric + qp_G = 0.5 * (P + P.T) # make sure P is symmetric qp_a = -q if A is not None: qp_C = -numpy.vstack([A, G]).T @@ -90,36 +118,62 @@ def quadprog_solve_qp(P, q, G=None, h=None, A=None, b=None): qp_C = -G.T qp_b = -h meq = 0 - return quadprog.solve_qp(qp_G.astype(np.float), qp_a.astype(np.float), qp_C.astype(np.float), qp_b.astype(np.float), meq)[0] + return quadprog.solve_qp( + qp_G.astype(np.float), + qp_a.astype(np.float), + qp_C.astype(np.float), + qp_b.astype(np.float), + meq, + )[0] -if __name__ == '__main__': + +if __name__ == "__main__": m = 30.9 r_g = [0, 0, 0.02] r_b = [0, 0, -0.05] - inertia = [[ 0.503217, 0.000204, -0.000526], - [ 0.000204, 0.893449, 0.000038], - [ -0.000526, 0.000038, 0.919819]] + inertia = [ + [0.503217, 0.000204, -0.000526], + [0.000204, 0.893449, 0.000038], + [-0.000526, 0.000038, 0.919819], + ] volume = 0.0295 - M_A = [[ 10.7727, 0, 0, 0, 0, 0], - [ 0, 10.7727, 0, 0, 0, 0], - [ 0, 0, 49.7679, 0, 0, 0], - [ 0, 0, 0, 1.0092, 0, 0], - [ 0, 0, 0, 0, 1.0092, 0], - [ 0, 0, 0, 0, 0, 0]] - D = [[ -9.5909, 0, 0, 0, 0, 0], - [ 0, -9.5909, 0, 0, 0, 0], - [ 0, 0, -50.5595, 0, 0, 0], - [ 0, 0, 0, -13.3040, 0, 0], - [ 0, 0, 0, 0, -13.3040, 0], - [ 0, 0, 0, 0, 0, -5.1559]] - auv_model = AUVModel(m, r_g, r_b, inertia, volume, M_A, D, rho=997, g=9.81, dot_eta_c=[0,0,0,0,0,0]) - - input_matrix = [[-0.707, 0.000, 0.000, 0.707, 0.707, 0.000, 0.000, -0.707], - [-0.707, 0.000, 0.000, -0.707, 0.707, 0.000, 0.000, 0.707], - [ 0.000, 1.000, 1.000, 0.000, 0.000, 1.000, 1.000, 0.000], - [ 0.046, -0.220, -0.220, 0.046, -0.046, 0.220, 0.220, -0.046], - [-0.046, -0.120, 0.120, 0.046, 0.046, 0.119, -0.119, -0.046], - [-0.324, 0.000, 0.000, 0.325, -0.325, 0.000, 0.000, 0.324]] + M_A = [ + [10.7727, 0, 0, 0, 0, 0], + [0, 10.7727, 0, 0, 0, 0], + [0, 0, 49.7679, 0, 0, 0], + [0, 0, 0, 1.0092, 0, 0], + [0, 0, 0, 0, 1.0092, 0], + [0, 0, 0, 0, 0, 0], + ] + D = [ + [-9.5909, 0, 0, 0, 0, 0], + [0, -9.5909, 0, 0, 0, 0], + [0, 0, -50.5595, 0, 0, 0], + [0, 0, 0, -13.3040, 0, 0], + [0, 0, 0, 0, -13.3040, 0], + [0, 0, 0, 0, 0, -5.1559], + ] + auv_model = AUVModel( + m, + r_g, + r_b, + inertia, + volume, + M_A, + D, + rho=997, + g=9.81, + dot_eta_c=[0, 0, 0, 0, 0, 0], + ) + + input_matrix = [ + [-0.707, 0.000, 0.000, 0.707, 0.707, 0.000, 0.000, -0.707], + [-0.707, 0.000, 0.000, -0.707, 0.707, 0.000, 0.000, 0.707], + [0.000, 1.000, 1.000, 0.000, 0.000, 1.000, 1.000, 0.000], + [0.046, -0.220, -0.220, 0.046, -0.046, 0.220, 0.220, -0.046], + [-0.046, -0.120, 0.120, 0.046, 0.046, 0.119, -0.119, -0.046], + [-0.324, 0.000, 0.000, 0.325, -0.325, 0.000, 0.000, 0.324], + ] rotor_time_constant = 0.2 u_max = 31.5 u_min = -31.5 @@ -138,10 +192,7 @@ def quadprog_solve_qp(P, q, G=None, h=None, A=None, b=None): t = 0 auv_simulator.set_initial_conditions(eta, nu, dot_nu, tau, t) - - - N = 50*5 + N = 50 * 5 h = 0.02 eta_ref = [1, 0, 0, 0, 0, 0] eta_d, nu_d, dot_nu_d = auv_simulator.generate_trajectory_for_dp(t, N, h, eta_ref) - diff --git a/motion/vtf_guidance_and_control/scripts/control_allocation.py b/motion/vtf_guidance_and_control/scripts/control_allocation.py index b844a12b9..53b39c9de 100755 --- a/motion/vtf_guidance_and_control/scripts/control_allocation.py +++ b/motion/vtf_guidance_and_control/scripts/control_allocation.py @@ -8,25 +8,43 @@ from functions import R_from_eul, T_from_eul import yaml + class ControlAllocationSystem: - def __init__(self, thruster_positions, thruster_orientations, rotor_time_constant, u_max, u_min, w): - self.input_matrix = compute_input_matrix(thruster_positions, thruster_orientations) + def __init__( + self, + thruster_positions, + thruster_orientations, + rotor_time_constant, + u_max, + u_min, + w, + ): + self.input_matrix = compute_input_matrix( + thruster_positions, thruster_orientations + ) self.rotor_time_constant = rotor_time_constant self.u_max = np.array(u_max) self.u_min = np.array(u_min) self.pseudo_inv_input_matrix = moore_penrose_pseudo_inverse(self.input_matrix) W = np.diag(w) - self.P = np.block([[W*1.000001, -W], [-W, W*1.000001]]) # Small cheat to make P positive definite + self.P = np.block( + [[W * 1.000001, -W], [-W, W * 1.000001]] + ) # Small cheat to make P positive definite self.c = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) - self.A = np.block([[self.pseudo_inv_input_matrix, -self.pseudo_inv_input_matrix], [-self.pseudo_inv_input_matrix, self.pseudo_inv_input_matrix]]) + self.A = np.block( + [ + [self.pseudo_inv_input_matrix, -self.pseudo_inv_input_matrix], + [-self.pseudo_inv_input_matrix, self.pseudo_inv_input_matrix], + ] + ) self.tau = np.array([0, 0, 0, 0, 0, 0]) - + def compute_control_inputs(self, tau_c): return np.dot(self.pseudo_inv_input_matrix, tau_c) - + def program_feasible_control_forces(self, tau_unb): u_unb = np.dot(self.pseudo_inv_input_matrix, tau_unb) - if any([x>self.u_max or x self.u_max or x < self.u_min for x in u_unb]): # If any unfeasible control inputs, generate feasible control forces using QP b = np.block([self.u_max - u_unb, -self.u_min + u_unb]) x = quadprog_solve_qp(self.P, self.c, self.A, b) @@ -36,10 +54,10 @@ def program_feasible_control_forces(self, tau_unb): else: tau_b = tau_unb self.tau = tau_b - ''' + """ tau_b[3] = 0 # Ensure no actuation in roll or pitch tau_b[4] = 0 - ''' + """ return tau_b def compute_optimized_control_inputs(self, tau_unb): @@ -49,9 +67,14 @@ def compute_optimized_control_inputs(self, tau_unb): def moore_penrose_pseudo_inverse(matrix): - return np.dot(np.array(matrix).T, np.linalg.inv(np.dot(np.array(matrix), np.array(matrix).T))) + return np.dot( + np.array(matrix).T, np.linalg.inv(np.dot(np.array(matrix), np.array(matrix).T)) + ) -def compute_input_matrix(thruster_positions, thruster_orientations, location_frame='ENU', output_frame='NED'): + +def compute_input_matrix( + thruster_positions, thruster_orientations, location_frame="ENU", output_frame="NED" +): r = len(thruster_positions) input_matrix = np.zeros((6, r)) F_p = np.array([1, 0, 0]) @@ -62,16 +85,17 @@ def compute_input_matrix(thruster_positions, thruster_orientations, location_fra R_offset = R_from_eul(eul_offset) T_offset = T_from_eul(eul_offset) for i in range(r): - p = np.dot(R_offset, thruster_positions[i,:]) - eul = np.dot(T_offset, thruster_orientations[i,:]) + p = np.dot(R_offset, thruster_positions[i, :]) + eul = np.dot(T_offset, thruster_orientations[i, :]) R = R_from_eul(eul) F_b = np.dot(R, F_p) - input_matrix[:3,i] = F_b - input_matrix[3:,i] = np.cross(p, F_b) + input_matrix[:3, i] = F_b + input_matrix[3:, i] = np.cross(p, F_b) return input_matrix + def quadprog_solve_qp(P, q, G=None, h=None, A=None, b=None): - qp_G = .5 * (P + P.T) # make sure P is symmetric + qp_G = 0.5 * (P + P.T) # make sure P is symmetric qp_a = -q if A is not None: qp_C = -numpy.vstack([A, G]).T @@ -81,19 +105,28 @@ def quadprog_solve_qp(P, q, G=None, h=None, A=None, b=None): qp_C = -G.T qp_b = -h meq = 0 - return quadprog.solve_qp(qp_G.astype(np.float), qp_a.astype(np.float), qp_C.astype(np.float), qp_b.astype(np.float), meq)[0] + return quadprog.solve_qp( + qp_G.astype(np.float), + qp_a.astype(np.float), + qp_C.astype(np.float), + qp_b.astype(np.float), + meq, + )[0] -if __name__ == '__main__': - with open('testing/beluga_test.yaml', 'r') as stream: + +if __name__ == "__main__": + with open("testing/beluga_test.yaml", "r") as stream: data = yaml.safe_load(stream) - thruster_positions = np.array(data['thrusters']['positions']) - thruster_orientations = np.array(data['thrusters']['orientations']) - rotor_time_constant = data['thrusters']['first_order_time_constant'] - W = data['guidance_and_control_system']['control_forces_weights'] + thruster_positions = np.array(data["thrusters"]["positions"]) + thruster_orientations = np.array(data["thrusters"]["orientations"]) + rotor_time_constant = data["thrusters"]["first_order_time_constant"] + W = data["guidance_and_control_system"]["control_forces_weights"] u_max = 10.5 u_min = -10.5 - control_allocation = ControlAllocationSystem(thruster_positions, thruster_orientations, rotor_time_constant, u_max, u_min, W) + control_allocation = ControlAllocationSystem( + thruster_positions, thruster_orientations, rotor_time_constant, u_max, u_min, W + ) tau_unb = [50, 20, 10, 0, 0, 10] u_unb = control_allocation.compute_control_inputs(tau_unb) u_sat = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) @@ -106,17 +139,11 @@ def quadprog_solve_qp(P, q, G=None, h=None, A=None, b=None): tau_b = control_allocation.program_feasible_control_forces(tau_unb) u_b = control_allocation.compute_control_inputs(tau_b) - data_tau = {'tau_unb': tau_unb, 'tau': tau, 'tau_b': tau_b} - data_u = {'u_unb': u_unb, 'u_b': u_b, 'u_sat': u_sat} + data_tau = {"tau_unb": tau_unb, "tau": tau, "tau_b": tau_b} + data_u = {"u_unb": u_unb, "u_b": u_b, "u_sat": u_sat} df1 = pd.DataFrame(data_tau) df2 = pd.DataFrame(data_u) - df1.to_csv('matlab/tau.csv') - df2.to_csv('matlab/u.csv') - - - - - - + df1.to_csv("matlab/tau.csv") + df2.to_csv("matlab/u.csv") diff --git a/motion/vtf_guidance_and_control/scripts/control_system.py b/motion/vtf_guidance_and_control/scripts/control_system.py index d6d44afba..9caa7ac41 100755 --- a/motion/vtf_guidance_and_control/scripts/control_system.py +++ b/motion/vtf_guidance_and_control/scripts/control_system.py @@ -6,74 +6,95 @@ from std_msgs.msg import Float32 import rospy -class DPControlSystem(): - '''Dynamic-positioning control system based on linear SISO models and a pole placement algorithm''' + +class DPControlSystem: + """Dynamic-positioning control system based on linear SISO models and a pole placement algorithm""" + def __init__(self, M, D, gvect, omega_b, zeta): self.M = M self.D = D self.gvect = gvect self.integral_terms = np.zeros(6) # Initialise the SISO controller - K_P = np.zeros((6,6)) - K_D = np.zeros((6,6)) - K_I = np.zeros((6,6)) - k = [0, 0, 0, 0, 0, 0] # Placeholder. Find spring coefficient for roll and pitch if it's desired to regulate these. + K_P = np.zeros((6, 6)) + K_D = np.zeros((6, 6)) + K_I = np.zeros((6, 6)) + k = [ + 0, + 0, + 0, + 0, + 0, + 0, + ] # Placeholder. Find spring coefficient for roll and pitch if it's desired to regulate these. for i in range(6): - K_P[i][i], K_D[i][i], K_I[i][i] = pid_pole_placement_algorithm(M[i][i], D[i][i], k[i], omega_b[i], zeta[i]) + K_P[i][i], K_D[i][i], K_I[i][i] = pid_pole_placement_algorithm( + M[i][i], D[i][i], k[i], omega_b[i], zeta[i] + ) self.controller = PIDController(K_P, K_D, K_I) - self.publisher_ = rospy.Publisher( - "yaw_error", Float32, queue_size=1 - ) + self.publisher_ = rospy.Publisher("yaw_error", Float32, queue_size=1) - def pid_regulate(self, eta, nu, eta_d, nu_d, dot_nu_d, t, dot_eta_c=[0,0,0,0,0,0]): + def pid_regulate( + self, eta, nu, eta_d, nu_d, dot_nu_d, t, dot_eta_c=[0, 0, 0, 0, 0, 0] + ): eta_error = np.array(eta) - np.array(eta_d) J = J_from_eul([0, 0, eta[5]]) eta_error_body = np.dot(J.T, eta_error) - J_target = J_from_eul([0, 0, eta[5]-eta_d[5]]) + J_target = J_from_eul([0, 0, eta[5] - eta_d[5]]) nu_d = np.dot(J_target.T, nu_d) - nu_error = (np.array(nu)-np.array(nu_d)) + nu_error = np.array(nu) - np.array(nu_d) self.publisher_.publish(nu_error[5]) - ''' + """ nu_c = np.dot(J.T, dot_eta_c) dot_v_c = np.dot(-skew([0, 0, nu[5]]), nu_c[:3]) dot_nu_c = np.hstack([dot_v_c, [0, 0, 0]]) - ''' + """ nu_c, dot_nu_c = current_to_body(eta, nu, dot_eta_c) tau_rff = np.zeros(6) tau_cff = np.zeros(6) for i in range(6): if i >= 3: eta_error_body[i] = ssa(eta_error_body[i]) - tau_rff[i] = self.M[i][i]*dot_nu_d[i] + self.D[i][i]*nu_d[i] + self.gvect[i] - tau_cff[i] = - self.M[i][i]*dot_nu_c[i] - self.D[i][i]*nu_c[i] - tau_pid, integral_terms = self.controller.pid_regulate(eta_error_body, nu_error, t) + tau_rff[i] = ( + self.M[i][i] * dot_nu_d[i] + self.D[i][i] * nu_d[i] + self.gvect[i] + ) + tau_cff[i] = -self.M[i][i] * dot_nu_c[i] - self.D[i][i] * nu_c[i] + tau_pid, integral_terms = self.controller.pid_regulate( + eta_error_body, nu_error, t + ) self.integral_terms = -np.array(integral_terms) + tau_cff return tau_cff + tau_rff + tau_pid - def pd_regulate(self, eta, nu, eta_d, nu_d, dot_nu_d, dot_eta_c=[0,0,0,0,0,0]): + def pd_regulate(self, eta, nu, eta_d, nu_d, dot_nu_d, dot_eta_c=[0, 0, 0, 0, 0, 0]): eta_error = np.array(eta) - np.array(eta_d) J = J_from_eul([0, 0, eta[5]]) eta_error_body = np.dot(J.T, eta_error) - J_target = J_from_eul([0, 0, eta[5]-eta_d[5]]) + J_target = J_from_eul([0, 0, eta[5] - eta_d[5]]) nu_d = np.dot(J_target.T, nu_d) - nu_error = np.array(nu)-np.array(nu_d) - ''' + nu_error = np.array(nu) - np.array(nu_d) + """ nu_c = np.dot(J.T, dot_eta_c) dot_v_c = np.dot(-skew([0, 0, nu[5]]), nu_c[:3]) dot_nu_c = np.hstack([dot_v_c, [0, 0, 0]]) - ''' + """ nu_c, dot_nu_c = current_to_body(eta, nu, dot_eta_c) tau_ff = np.zeros(6) for i in range(6): if i >= 3: eta_error_body[i] = ssa(eta_error_body[i]) - tau_ff[i] = self.M[i][i]*(dot_nu_d[i]-dot_nu_c[i]) + self.D[i][i]*(nu_d[i]-nu_c[i]) + self.gvect[i] + tau_ff[i] = ( + self.M[i][i] * (dot_nu_d[i] - dot_nu_c[i]) + + self.D[i][i] * (nu_d[i] - nu_c[i]) + + self.gvect[i] + ) tau_pd = self.controller.pd_regulate(eta_error_body, nu_error) return tau_ff + tau_pd + class PIDController: """Multidimensional PID controller""" + def __init__(self, K_P, K_D, K_I, u_max=np.inf): """Initialize the PID controller @@ -97,15 +118,19 @@ def initialize(self, t): def pid_regulate(self, x_err, dot_x_err, t): dt = t - self.prev_t if self.prev_t > 0.0 and dt > 0.0: - self.integral += np.array(x_err)*dt - u_unsat = - (np.dot(self.K_P, x_err) + np.dot(self.K_D, dot_x_err) + np.dot(self.K_I, self.integral)) + self.integral += np.array(x_err) * dt + u_unsat = -( + np.dot(self.K_P, x_err) + + np.dot(self.K_D, dot_x_err) + + np.dot(self.K_I, self.integral) + ) self.prev_t = t u = np.zeros(len(u_unsat)) for i in range(len(u_unsat)): if abs(u_unsat[i]) > self.u_max: # Anti-wind-up u[i] = np.sign(u_unsat[i]) * self.u_max - self.integral[i] += - (dt/self.K_I[i][i]) * (u[i] - u_unsat[i]) + self.integral[i] += -(dt / self.K_I[i][i]) * (u[i] - u_unsat[i]) else: u[i] = u_unsat[i] return u, (np.dot(self.K_I, self.integral)) @@ -113,19 +138,12 @@ def pid_regulate(self, x_err, dot_x_err, t): def pd_regulate(self, x_err, dot_x_err): return -(np.dot(self.K_P, x_err) + np.dot(self.K_D, dot_x_err)) + def pid_pole_placement_algorithm(m, d, k, omega_b, zeta): - omega_n = omega_b/np.sqrt(1 - 2*zeta**2+np.sqrt(4*zeta**4 - 4*zeta**2 + 2)) - k_p = m*omega_n**2 - k - k_d = 2*zeta*omega_n*m - d - k_i = omega_n*k_p/7 + omega_n = omega_b / np.sqrt( + 1 - 2 * zeta**2 + np.sqrt(4 * zeta**4 - 4 * zeta**2 + 2) + ) + k_p = m * omega_n**2 - k + k_d = 2 * zeta * omega_n * m - d + k_i = omega_n * k_p / 7 return k_p, k_d, k_i - - - - - - - - - - diff --git a/motion/vtf_guidance_and_control/scripts/current_estimator.py b/motion/vtf_guidance_and_control/scripts/current_estimator.py index 52df9ba58..9470d6d6c 100755 --- a/motion/vtf_guidance_and_control/scripts/current_estimator.py +++ b/motion/vtf_guidance_and_control/scripts/current_estimator.py @@ -5,6 +5,7 @@ import numpy as np from functions import ssa + class CurrentEstimator: def __init__(self, delta_psi, X_u, Y_v): self.delta_psi = delta_psi @@ -12,9 +13,16 @@ def __init__(self, delta_psi, X_u, Y_v): self.Y_v = Y_v self.tol_error = 0.05 self.tol_sec = 5 - self.data_est = {'U_c': [], 'chi_c': [], 'U_c_mean': [], 'chi_c_mean': [], 'psi': [], 't': []} + self.data_est = { + "U_c": [], + "chi_c": [], + "U_c_mean": [], + "chi_c_mean": [], + "psi": [], + "t": [], + } self.online = False - + def initailize(self, eta_r, t): self.eta_r = eta_r self.dot_x_c = 0 @@ -26,7 +34,9 @@ def initailize(self, eta_r, t): self.online = True def check_steady_state(self, eta, t): - if (np.linalg.norm(eta - np.array(self.eta_r)) < self.tol_error) and (t-self.prev_t > self.tol_sec): + if (np.linalg.norm(eta - np.array(self.eta_r)) < self.tol_error) and ( + t - self.prev_t > self.tol_sec + ): self.prev_t = t result = True elif np.linalg.norm(eta - np.array(self.eta_r)) < self.tol_error: @@ -35,27 +45,39 @@ def check_steady_state(self, eta, t): self.prev_t = t result = False return result - + def estimate_current_velocity(self, eta, N, int_x, int_y, t): if self.check_steady_state(eta, t): if self.i > 0: - delta_int_x = int_x - self.prev_int_x + delta_int_x = int_x - self.prev_int_x delta_int_y = int_y - self.prev_int_y - new_U_c = (1/(2*np.sin(self.delta_psi/2)))*np.sqrt((delta_int_x/(self.X_u))**2 + (delta_int_y/(self.Y_v))**2) - self.U_c = (self.U_c*(self.i-1) + new_U_c)/self.i - sign = -np.sign(delta_int_x)*np.sign(self.delta_psi) - new_chi_c = ssa(sign*np.arccos((delta_int_y/(self.Y_v))/(np.sqrt((delta_int_x/(self.X_u))**2 + (delta_int_y/(self.Y_v))**2))) + self.eta_r[5] - self.delta_psi/2) - self.chi_c = (self.chi_c*(self.i-1) + new_chi_c)/self.i + new_U_c = (1 / (2 * np.sin(self.delta_psi / 2))) * np.sqrt( + (delta_int_x / (self.X_u)) ** 2 + (delta_int_y / (self.Y_v)) ** 2 + ) + self.U_c = (self.U_c * (self.i - 1) + new_U_c) / self.i + sign = -np.sign(delta_int_x) * np.sign(self.delta_psi) + new_chi_c = ssa( + sign + * np.arccos( + (delta_int_y / (self.Y_v)) + / ( + np.sqrt( + (delta_int_x / (self.X_u)) ** 2 + + (delta_int_y / (self.Y_v)) ** 2 + ) + ) + ) + + self.eta_r[5] + - self.delta_psi / 2 + ) + self.chi_c = (self.chi_c * (self.i - 1) + new_chi_c) / self.i self.prev_int_x = int_x self.prev_int_y = int_y - self.dot_x_c = self.U_c*np.cos(self.chi_c) - self.dot_y_c = self.U_c*np.sin(self.chi_c) + self.dot_x_c = self.U_c * np.cos(self.chi_c) + self.dot_y_c = self.U_c * np.sin(self.chi_c) if self.i == N: self.online = False else: self.eta_r[5] += self.delta_psi self.eta_r[5] = ssa(self.eta_r[5]) self.i += 1 - - - diff --git a/motion/vtf_guidance_and_control/scripts/functions.py b/motion/vtf_guidance_and_control/scripts/functions.py index 86bdda303..227807fab 100755 --- a/motion/vtf_guidance_and_control/scripts/functions.py +++ b/motion/vtf_guidance_and_control/scripts/functions.py @@ -4,10 +4,11 @@ import numpy as np from functools import wraps + def get_gvect(m, g, r_g, r_b, rho, volume, eul): gvect = np.zeros(6) - W = m*g - B = rho*g*volume + W = m * g + B = rho * g * volume R = R_from_eul(eul) f_g = np.dot(R.T, [0, 0, W]) f_b = np.dot(R.T, [0, 0, -B]) @@ -15,47 +16,86 @@ def get_gvect(m, g, r_g, r_b, rho, volume, eul): gvect[3:] = np.cross(r_g, f_g) + np.cross(r_b, f_b) return -gvect + def get_M_RB(m, r_g, inertia): - M_11 = m*np.eye(3) - M_12 = -m*skew(r_g) - M_21 = m*skew(r_g) - M_22 = np.array(inertia) - m*skew(r_g)**2 + M_11 = m * np.eye(3) + M_12 = -m * skew(r_g) + M_21 = m * skew(r_g) + M_22 = np.array(inertia) - m * skew(r_g) ** 2 return np.block([[M_11, M_12], [M_21, M_22]]) + def skew(v): return np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]]) + def R_x(rot): - return np.array([[1, 0, 0], - [0, np.cos(rot), -np.sin(rot)], - [0, np.sin(rot), np.cos(rot)]]) + return np.array( + [[1, 0, 0], [0, np.cos(rot), -np.sin(rot)], [0, np.sin(rot), np.cos(rot)]] + ) + def R_y(rot): - return np.array([[np.cos(rot), 0, np.sin(rot)], - [0, 1, 0], - [-np.sin(rot), 0, np.cos(rot)]]) + return np.array( + [[np.cos(rot), 0, np.sin(rot)], [0, 1, 0], [-np.sin(rot), 0, np.cos(rot)]] + ) + def R_z(rot): - return np.array([[np.cos(rot), -np.sin(rot), 0], - [np.sin(rot), np.cos(rot), 0], - [0, 0, 1]]) + return np.array( + [[np.cos(rot), -np.sin(rot), 0], [np.sin(rot), np.cos(rot), 0], [0, 0, 1]] + ) + def R_path(chi_p, gamma_p): return np.dot(R_y(gamma_p), R_z(chi_p)) + def R_from_eul(eul): - return np.array([[np.cos(eul[2])*np.cos(eul[1]), -np.sin(eul[2])*np.cos(eul[0]) + np.cos(eul[2])*np.sin(eul[1])*np.sin(eul[0]), np.sin(eul[2])*np.sin(eul[0]) + np.cos(eul[2])*np.cos(eul[0])*np.sin(eul[1])], - [np.sin(eul[2])*np.cos(eul[1]), np.cos(eul[2])*np.cos(eul[0]) + np.sin(eul[0])*np.sin(eul[1])*np.sin(eul[2]), -np.cos(eul[2])*np.sin(eul[0]) + np.sin(eul[1])*np.sin(eul[2])*np.cos(eul[0])], - [-np.sin(eul[1]), np.cos(eul[1])*np.sin(eul[0]), np.cos(eul[1])*np.cos(eul[0])]]) + return np.array( + [ + [ + np.cos(eul[2]) * np.cos(eul[1]), + -np.sin(eul[2]) * np.cos(eul[0]) + + np.cos(eul[2]) * np.sin(eul[1]) * np.sin(eul[0]), + np.sin(eul[2]) * np.sin(eul[0]) + + np.cos(eul[2]) * np.cos(eul[0]) * np.sin(eul[1]), + ], + [ + np.sin(eul[2]) * np.cos(eul[1]), + np.cos(eul[2]) * np.cos(eul[0]) + + np.sin(eul[0]) * np.sin(eul[1]) * np.sin(eul[2]), + -np.cos(eul[2]) * np.sin(eul[0]) + + np.sin(eul[1]) * np.sin(eul[2]) * np.cos(eul[0]), + ], + [ + -np.sin(eul[1]), + np.cos(eul[1]) * np.sin(eul[0]), + np.cos(eul[1]) * np.cos(eul[0]), + ], + ] + ) + def T_from_eul(eul): - return np.array([[1, np.sin(eul[0])*np.tan(eul[1]), np.cos(eul[0])*np.tan(eul[1])], [0, np.cos(eul[0]), -np.sin(eul[0])], [0, np.sin(eul[0])/np.cos(eul[1]), np.cos(eul[0])]/np.cos(eul[1])]) + return np.array( + [ + [1, np.sin(eul[0]) * np.tan(eul[1]), np.cos(eul[0]) * np.tan(eul[1])], + [0, np.cos(eul[0]), -np.sin(eul[0])], + [0, np.sin(eul[0]) / np.cos(eul[1]), np.cos(eul[0])] / np.cos(eul[1]), + ] + ) + def J_from_eul(eul): - return np.block([[R_from_eul(eul), np.zeros((3,3))], [np.zeros((3,3)), T_from_eul(eul)]]) + return np.block( + [[R_from_eul(eul), np.zeros((3, 3))], [np.zeros((3, 3)), T_from_eul(eul)]] + ) + def euler2(dot_x, x, h): - return x + dot_x*h + return x + dot_x * h + def ssa(angle): if angle < -np.pi or angle >= np.pi: @@ -63,12 +103,14 @@ def ssa(angle): angle = (angle + np.pi) % (2 * np.pi) - np.pi return angle + def inside_sphere_of_acceptence(pos, goal, pos_tol): - if (np.linalg.norm(np.array(pos)-np.array(goal)) < pos_tol): + if np.linalg.norm(np.array(pos) - np.array(goal)) < pos_tol: return True else: return False + def current_to_body(eta, nu, dot_eta_c): J = J_from_eul([0, 0, eta[5]]) nu_c = np.dot(J.T, dot_eta_c) @@ -76,8 +118,10 @@ def current_to_body(eta, nu, dot_eta_c): dot_nu_c = np.hstack([dot_v_c, [0, 0, 0]]) return nu_c, dot_nu_c + def memoize(function): memo = {} + @wraps(function) def wrapper(*args): try: @@ -86,20 +130,27 @@ def wrapper(*args): rv = function(*args) memo[args] = rv return rv + return wrapper -def ned_enu_conversion(eta,nu): + +def ned_enu_conversion(eta, nu): """Rotates from north-east-down to east-north-up - - Args: + + Args: eta (array) : Drone position and rotation nu (array) : Twitch of the drone - angular and linear velocities - + Returns: Array: Rotated eta and nu - + """ # yaw velocity is wrong -> ask aksel why! - return [eta[0], eta[1], eta[2], eta[3], eta[4], eta[5]], [nu[0], nu[1], nu[2], nu[3], nu[4], -nu[5]] - - \ No newline at end of file + return [eta[0], eta[1], eta[2], eta[3], eta[4], eta[5]], [ + nu[0], + nu[1], + nu[2], + nu[3], + nu[4], + -nu[5], + ] diff --git a/motion/vtf_guidance_and_control/scripts/guidance_and_control_node.py b/motion/vtf_guidance_and_control/scripts/guidance_and_control_node.py index 4d383128c..481d41fbc 100755 --- a/motion/vtf_guidance_and_control/scripts/guidance_and_control_node.py +++ b/motion/vtf_guidance_and_control/scripts/guidance_and_control_node.py @@ -19,26 +19,35 @@ from functions import inside_sphere_of_acceptence, ssa, ned_enu_conversion from control_system import PIDController + class VtfGuidanceAndControlNode: def __init__(self): - rospy.Subscriber(rospy.get_param("/controllers/vtf/odometry_topic"), Odometry, self.navigation_callback) + rospy.Subscriber( + rospy.get_param("/controllers/vtf/odometry_topic"), + Odometry, + self.navigation_callback, + ) + + self.pub = rospy.Publisher( + rospy.get_param("/thrust/thrust_topic"), Wrench, queue_size=1 + ) - self.pub = rospy.Publisher(rospy.get_param("/thrust/thrust_topic"), Wrench, queue_size=1) - - self.path_pub = rospy.Publisher('path', Path, queue_size=1) #Path publisher -> probably change that to path planner when that is created + self.path_pub = rospy.Publisher( + "path", Path, queue_size=1 + ) # Path publisher -> probably change that to path planner when that is created self.br = tf.TransformBroadcaster() self.br_eta_r = tf.TransformBroadcaster() self.get_pose = False - '''Initial states''' + """Initial states""" self.eta_d = [0, 0, 0.5, 0, 0, 0] self.nu_d = [0, 0, 0, 0, 0, 0] self.dot_eta_c = [0, 0, 0, 0, 0, 0] - self.mode = 'path_following' + self.mode = "path_following" - '''Initialize Fossen's equations''' + """Initialize Fossen's equations""" m = rospy.get_param("/controllers/vtf/model_parameters/mass") r_g = rospy.get_param("/controllers/vtf/model_parameters/center_of_mass") r_b = rospy.get_param("/controllers/vtf/model_parameters/center_of_buoyancy") @@ -50,59 +59,105 @@ def __init__(self): g = 9.81 self.auv_model = AUVModel(m, r_g, r_b, inertia, volume, M_A, D, rho=rho, g=g) - '''Initialize control allocation system''' - thruster_positions = np.array(rospy.get_param("/controllers/vtf/thruster_parameters/positions")) - thruster_orientations = np.array(rospy.get_param("/controllers/vtf/thruster_parameters/orientations")) - rotor_time_constant = rospy.get_param("/controllers/vtf/thruster_parameters/first_order_time_constant") + """Initialize control allocation system""" + thruster_positions = np.array( + rospy.get_param("/controllers/vtf/thruster_parameters/positions") + ) + thruster_orientations = np.array( + rospy.get_param("/controllers/vtf/thruster_parameters/orientations") + ) + rotor_time_constant = rospy.get_param( + "/controllers/vtf/thruster_parameters/first_order_time_constant" + ) w = rospy.get_param("/controllers/vtf/control_forces_weights") u_min = rospy.get_param("/controllers/vtf/control_input_saturation_limits")[0] u_max = rospy.get_param("/controllers/vtf/control_input_saturation_limits")[1] - self.control_allocation_system = ControlAllocationSystem(thruster_positions, thruster_orientations, rotor_time_constant, u_max, u_min, w) + self.control_allocation_system = ControlAllocationSystem( + thruster_positions, + thruster_orientations, + rotor_time_constant, + u_max, + u_min, + w, + ) - '''Initialize DP control system''' + """Initialize DP control system""" M = self.auv_model.M D = self.auv_model.D gvect = self.auv_model.gvect omega_b = np.array(rospy.get_param("/controllers/vtf/control_bandwidth")) zeta = np.array(rospy.get_param("/controllers/vtf/relative_damping_ratio")) self.dp_control_system = DPControlSystem(M, D, gvect, omega_b, zeta) - '''Initialize reference model''' - u_gain = rospy.get_param("/controllers/vtf/reference_model_control_input_saturation_limit_gain") - u_min_simulator = u_min*u_gain - u_max_simulator = u_max*u_gain - simulator_control_allocation_system = ControlAllocationSystem(thruster_positions, thruster_orientations, rotor_time_constant, u_max_simulator, u_min_simulator, w) - omega_b_gain = rospy.get_param("/controllers/vtf/reference_model_control_bandwidth_gain") - self.omega_b_simulator = [x*omega_b_gain for x in omega_b] - zeta = [1 ,1, 1, 1, 1, 1] - simulator_control_system = DPControlSystem(M, D, gvect, self.omega_b_simulator, zeta) - absolute_relative_velocity_limit = rospy.get_param("/controllers/vtf/absolute_relative_velocity_limit") - self.reference_model = AUVSimulator(self.auv_model, simulator_control_allocation_system, simulator_control_system, absolute_relative_velocity_limit) - - '''Initialize path-following controller''' - u_gain = rospy.get_param("/controllers/vtf/virtual_target_control_input_saturation_limit_gain") - u_min_vt= u_min*u_gain - u_max_vt = u_max*u_gain - self.vt_actuator_model = ControlAllocationSystem(thruster_positions, thruster_orientations, rotor_time_constant, u_max_vt, u_min_vt, w) - self.waypoints = [] - self.path_following_controller = None - self.heading_mode = 'path_dependent_heading' # Use either 'path_dependent_heading' or 'point_dependent_heading' - self.dot_s_bounds = rospy.get_param("/controllers/vtf/virtual_target_along_track_speed_saturation_limits") - self.heading_point = [0,0] - - '''Virtual target position and speed''' - self.eta_r = [0,0,0,0,0,0] + """Initialize reference model""" + u_gain = rospy.get_param( + "/controllers/vtf/reference_model_control_input_saturation_limit_gain" + ) + u_min_simulator = u_min * u_gain + u_max_simulator = u_max * u_gain + simulator_control_allocation_system = ControlAllocationSystem( + thruster_positions, + thruster_orientations, + rotor_time_constant, + u_max_simulator, + u_min_simulator, + w, + ) + omega_b_gain = rospy.get_param( + "/controllers/vtf/reference_model_control_bandwidth_gain" + ) + self.omega_b_simulator = [x * omega_b_gain for x in omega_b] + zeta = [1, 1, 1, 1, 1, 1] + simulator_control_system = DPControlSystem( + M, D, gvect, self.omega_b_simulator, zeta + ) + absolute_relative_velocity_limit = rospy.get_param( + "/controllers/vtf/absolute_relative_velocity_limit" + ) + self.reference_model = AUVSimulator( + self.auv_model, + simulator_control_allocation_system, + simulator_control_system, + absolute_relative_velocity_limit, + ) + + """Initialize path-following controller""" + u_gain = rospy.get_param( + "/controllers/vtf/virtual_target_control_input_saturation_limit_gain" + ) + u_min_vt = u_min * u_gain + u_max_vt = u_max * u_gain + self.vt_actuator_model = ControlAllocationSystem( + thruster_positions, + thruster_orientations, + rotor_time_constant, + u_max_vt, + u_min_vt, + w, + ) + self.waypoints = [] + self.path_following_controller = None + self.heading_mode = "path_dependent_heading" # Use either 'path_dependent_heading' or 'point_dependent_heading' + self.dot_s_bounds = rospy.get_param( + "/controllers/vtf/virtual_target_along_track_speed_saturation_limits" + ) + self.heading_point = [0, 0] + + """Virtual target position and speed""" + self.eta_r = [0, 0, 0, 0, 0, 0] self.dot_s = 0 - '''Sphere of acceptance''' + """Sphere of acceptance""" self.goal_reached = False - self.sphere_of_acceptance_radius = rospy.get_param("/controllers/vtf/sphere_of_acceptence") + self.sphere_of_acceptance_radius = rospy.get_param( + "/controllers/vtf/sphere_of_acceptence" + ) - '''Publish frequency''' + """Publish frequency""" self.publish_rate = rospy.get_param("/controllers/vtf/publish_rate") self.rate = rospy.Rate(self.publish_rate) - '''TF listeners''' - self.tf_buffer = tf2_ros.Buffer(rospy.Duration(1200.0)) #tf buffer length + """TF listeners""" + self.tf_buffer = tf2_ros.Buffer(rospy.Duration(1200.0)) # tf buffer length self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) def vtf_reconfigure(self, config, level): @@ -121,7 +176,9 @@ def vtf_reconfigure(self, config, level): def navigation_callback(self, msg): if self.get_pose: - self.eta, self.nu = ned_enu_conversion(extract_from_pose(msg.pose.pose),extract_from_twist(msg.twist.twist)) + self.eta, self.nu = ned_enu_conversion( + extract_from_pose(msg.pose.pose), extract_from_twist(msg.twist.twist) + ) self.get_pose = False else: pass @@ -134,62 +191,116 @@ def get_state_estimates(self): def new_path_recieved(self, speed, heading, heading_point): self.get_state_estimates() self.reference_model.set_initial_conditions(self.eta, self.nu, rospy.get_time()) - self.waypoints[0] = [self.eta[0],self.eta[1],self.eta[2]] #First waypoint is current location + self.waypoints[0] = [ + self.eta[0], + self.eta[1], + self.eta[2], + ] # First waypoint is current location path = VTFPath() path.generate_G0_path(self.waypoints) - omega_b_virtual = rospy.get_param("/controllers/vtf/virtual_target_controller_bandwidths") - virtual_control_system = DPControlSystem(self.auv_model.M, self.auv_model.D, self.auv_model.gvect, omega_b_virtual, [1, 1, 1, 1, 1, 1]) - - self.dot_s_bounds = [-speed,speed] + omega_b_virtual = rospy.get_param( + "/controllers/vtf/virtual_target_controller_bandwidths" + ) + virtual_control_system = DPControlSystem( + self.auv_model.M, + self.auv_model.D, + self.auv_model.gvect, + omega_b_virtual, + [1, 1, 1, 1, 1, 1], + ) + + self.dot_s_bounds = [-speed, speed] self.heading_mode = heading self.heading_point = heading_point - - self.path_following_controller = VirtualTarget(path, self.auv_model, self.vt_actuator_model, virtual_control_system, self.omega_b_simulator, dot_s_bounds=self.dot_s_bounds) - - self.publish_path_once(path) #Publishes path -> probably move to pathplanner node once that is created - + + self.path_following_controller = VirtualTarget( + path, + self.auv_model, + self.vt_actuator_model, + virtual_control_system, + self.omega_b_simulator, + dot_s_bounds=self.dot_s_bounds, + ) + + self.publish_path_once( + path + ) # Publishes path -> probably move to pathplanner node once that is created + def update_path(self, speed, heading, heading_point): - - self.waypoints[0] = [self.eta_r[0],self.eta_r[1],self.eta_r[2]] #First waypoint is current location + + self.waypoints[0] = [ + self.eta_r[0], + self.eta_r[1], + self.eta_r[2], + ] # First waypoint is current location path = VTFPath() path.generate_G0_path(self.waypoints) - + self.path_following_controller.varpi = 0 self.path_following_controller.path = path - + self.publish_path_once(path) - + def publish_control_forces(self): self.get_state_estimates() # Path following mode - if self.mode == 'path_following': + if self.mode == "path_following": final_wp = self.path_following_controller.path.path[-1](1) - final_orientation = [0, 0, 0] - if inside_sphere_of_acceptence(self.eta[:3], final_wp, self.sphere_of_acceptance_radius): - nu_r = np.zeros(6) + final_orientation = [0, 0, 0] + if inside_sphere_of_acceptence( + self.eta[:3], final_wp, self.sphere_of_acceptance_radius + ): + nu_r = np.zeros(6) eta_r = np.zeros(6) eta_r[:3] = final_wp eta_r[3:] = final_orientation self.goal_reached = True else: - eta_r, nu_r, self.dot_s = self.path_following_controller.generate_reference_trajectories(self.eta_d, self.nu_d, rospy.get_time(), self.heading_mode, point=self.heading_point) + ( + eta_r, + nu_r, + self.dot_s, + ) = self.path_following_controller.generate_reference_trajectories( + self.eta_d, + self.nu_d, + rospy.get_time(), + self.heading_mode, + point=self.heading_point, + ) - # Pose hold mode -> Currently not used! - elif self.mode == 'pose_hold': - eta_r = [0, 0, 0, 0, 0, 0] # Insert desired pose here (roll and pitch do not work atm) [x, y, z, roll, pitch, yaw] NED + # Pose hold mode -> Currently not used! + elif self.mode == "pose_hold": + eta_r = [ + 0, + 0, + 0, + 0, + 0, + 0, + ] # Insert desired pose here (roll and pitch do not work atm) [x, y, z, roll, pitch, yaw] NED nu_r = [0, 0, 0, 0, 0, 0] - - # Reference model if not self.reference_model.online: - self.reference_model.set_initial_conditions(self.eta, self.nu, rospy.get_time()) - eta_d, nu_d, dot_nu_d = self.reference_model.generate_trajectory_for_dp(rospy.get_time(), 1, 1/float(self.publish_rate), eta_r, nu_ref=nu_r) + self.reference_model.set_initial_conditions( + self.eta, self.nu, rospy.get_time() + ) + eta_d, nu_d, dot_nu_d = self.reference_model.generate_trajectory_for_dp( + rospy.get_time(), 1, 1 / float(self.publish_rate), eta_r, nu_ref=nu_r + ) self.eta_d = eta_d[0] self.nu_d = nu_d[0] # Control System - tau_c = self.dp_control_system.pid_regulate(self.eta, self.nu, eta_d[0], nu_d[0], [0,0,0,0,0,0], rospy.get_time(), dot_eta_c=self.dot_eta_c) + tau_c = self.dp_control_system.pid_regulate( + self.eta, + self.nu, + eta_d[0], + nu_d[0], + [0, 0, 0, 0, 0, 0], + rospy.get_time(), + dot_eta_c=self.dot_eta_c, + ) # Save virtual target position self.eta_r = eta_r @@ -198,36 +309,40 @@ def publish_control_forces(self): p = eta_r[:3] eul = eta_r[3:] q = quaternion_from_euler(eul[0], eul[1], eul[2]) - self.br.sendTransform((p[0], p[1], p[2]), - (q[0], q[1], q[2], q[3]), - rospy.Time.now(), - "/virtual_target", - "/odom") - + self.br.sendTransform( + (p[0], p[1], p[2]), + (q[0], q[1], q[2], q[3]), + rospy.Time.now(), + "/virtual_target", + "/odom", + ) + # Publish reference model frame p = eta_d[0][:3] eul = eta_d[0][3:] q = quaternion_from_euler(eul[0], eul[1], eul[2]) - self.br_eta_r.sendTransform((p[0], p[1], p[2]), - (q[0], q[1], q[2], q[3]), - rospy.Time.now(), - "/reference_model", - "/odom") - + self.br_eta_r.sendTransform( + (p[0], p[1], p[2]), + (q[0], q[1], q[2], q[3]), + rospy.Time.now(), + "/reference_model", + "/odom", + ) + # Publish control forces msg = create_wrench_msg(tau_c) self.pub.publish(msg) - - def publish_path_once(self,path): + + def publish_path_once(self, path): msg = Path() - msg.header.frame_id = '/odom' + msg.header.frame_id = "/odom" for i in range(len(path.path)): for j in list(np.linspace(0, 1, 50)): p = path.path[i](j) psi = path.chi_p[i](j) q = quaternion_from_euler(0, 0, psi) pose = PoseStamped() - pose.header.frame_id = '/odom' + pose.header.frame_id = "/odom" pose.pose.position.x = p[0] pose.pose.position.y = p[1] pose.pose.position.z = p[2] @@ -236,15 +351,25 @@ def publish_path_once(self,path): pose.pose.orientation.z = q[2] pose.pose.orientation.w = q[3] msg.poses.append(pose) - + self.path_pub.publish(msg) - + def extract_from_pose(pose): quaternions = pose.orientation - euler_angles = euler_from_quaternion([quaternions.x, quaternions.y, quaternions.z, quaternions.w]) + euler_angles = euler_from_quaternion( + [quaternions.x, quaternions.y, quaternions.z, quaternions.w] + ) position = (pose.position.x, pose.position.y, pose.position.z) - return [position[0], position[1], position[2], euler_angles[0], euler_angles[1], euler_angles[2]] + return [ + position[0], + position[1], + position[2], + euler_angles[0], + euler_angles[1], + euler_angles[2], + ] + def extract_from_twist(twist): linear = (twist.linear.x, twist.linear.y, twist.linear.z) @@ -261,6 +386,3 @@ def create_wrench_msg(tau): msg.torque.y = tau[4] msg.torque.z = tau[5] return msg - - - diff --git a/motion/vtf_guidance_and_control/scripts/path.py b/motion/vtf_guidance_and_control/scripts/path.py index 94325f005..aa8c41826 100755 --- a/motion/vtf_guidance_and_control/scripts/path.py +++ b/motion/vtf_guidance_and_control/scripts/path.py @@ -1,4 +1,3 @@ - #!/usr/bin/python3 # Written by Aksel Kristoffersen # Documentation can be found in my master's thesis: Guidance and Control System for Dynamic Positioning and PathFollowing of an AUV exposed to Ocean Currents @@ -7,6 +6,7 @@ import pandas as pd from functions import R_z, ssa + class Path: def __init__(self): self.path = [] @@ -17,24 +17,24 @@ def __init__(self): self.length = [] def generate_G0_path(self, waypoints): - for i in range(len(waypoints)-1): - v = [a - b for a, b in zip(waypoints[i+1], waypoints[i])] + for i in range(len(waypoints) - 1): + v = [a - b for a, b in zip(waypoints[i + 1], waypoints[i])] l = np.linalg.norm(v) chi_l = np.arctan2(v[1], v[0]) - gamma_l = -np.arctan2(v[2] , np.linalg.norm(v[:2])) + gamma_l = -np.arctan2(v[2], np.linalg.norm(v[:2])) self.straight_line_path_segment(waypoints[i], l, chi_l, gamma_l) def generate_G1_path(self, waypoints, r_h, r_v): wp_0 = waypoints[0] - for i in range(len(waypoints)-2): - v1 = [a - b for a, b in zip(waypoints[i+1], wp_0)] - v2 = [a - b for a, b in zip(waypoints[i+2], waypoints[i+1])] + for i in range(len(waypoints) - 2): + v1 = [a - b for a, b in zip(waypoints[i + 1], wp_0)] + v2 = [a - b for a, b in zip(waypoints[i + 2], waypoints[i + 1])] chi_0 = np.arctan2(v1[1], v1[0]) - gamma_0 = -np.arctan2(v1[2] , np.linalg.norm(v1[:2])) + gamma_0 = -np.arctan2(v1[2], np.linalg.norm(v1[:2])) chi_1 = np.arctan2(v2[1], v2[0]) - gamma_1 = -np.arctan2(v2[2] , np.linalg.norm(v2[:2])) - ''' + gamma_1 = -np.arctan2(v2[2], np.linalg.norm(v2[:2])) + """ #alpha_h = -(np.arctan2(v1[0]*v2[1]-v1[1]*v2[0], v1[0]*v2[0]-v1[1]*v2[1]))/2 alpha_h = (chi_1-chi_0+np.pi)/2 alpha_v = (gamma_1-gamma_0+np.pi)/2 @@ -45,77 +45,116 @@ def generate_G1_path(self, waypoints, r_h, r_v): cut_v = r_v / np.tan(alpha_v) tot_cut = np.sqrt(cut_h**2 + cut_v**2) l_1 = np.linalg.norm(v1)-tot_cut - ''' - #alpha_h = -(np.arctan2(v1[0]*v2[1]-v1[1]*v2[0], v1[0]*v2[0]-v1[1]*v2[1]))/2 - alpha_h = (chi_1-chi_0+np.pi)/2 - #v2_v = v2[2]*np.linalg.norm(v2[:2]) - #alpha_v = (np.arctan2(v1_v[0]*v2_v[1]-v1_v[1]*v2_v[0], v1_v[0]*v2_v[0]-v1_v[1]*v2_v[1]))/2 + """ + # alpha_h = -(np.arctan2(v1[0]*v2[1]-v1[1]*v2[0], v1[0]*v2[0]-v1[1]*v2[1]))/2 + alpha_h = (chi_1 - chi_0 + np.pi) / 2 + # v2_v = v2[2]*np.linalg.norm(v2[:2]) + # alpha_v = (np.arctan2(v1_v[0]*v2_v[1]-v1_v[1]*v2_v[0], v1_v[0]*v2_v[0]-v1_v[1]*v2_v[1]))/2 cut_h = r_h / np.tan(alpha_h) - tot_cut = np.sqrt(cut_h**2 + (cut_h*np.sin(gamma_0))**2) - l_1 = np.linalg.norm(v1)-tot_cut + tot_cut = np.sqrt(cut_h**2 + (cut_h * np.sin(gamma_0)) ** 2) + l_1 = np.linalg.norm(v1) - tot_cut self.straight_line_path_segment(wp_0, l_1, chi_0, gamma_0) wp_1 = self.path[-1](1) - c = wp_1 + np.sign(ssa(chi_1-chi_0))*np.dot(R_z(chi_0), [0, r_h, 0]) - c_v = wp_1[2] - np.sign(gamma_1-gamma_0)*np.cos(gamma_0)*r_v - c[2] = waypoints[i+1][2] - #print(c[0]+np.sin(chi_1)*r_h) - #print(c[1]-np.cos(chi_1)*r_h) - #print(c) + c = wp_1 + np.sign(ssa(chi_1 - chi_0)) * np.dot(R_z(chi_0), [0, r_h, 0]) + c_v = wp_1[2] - np.sign(gamma_1 - gamma_0) * np.cos(gamma_0) * r_v + c[2] = waypoints[i + 1][2] + # print(c[0]+np.sin(chi_1)*r_h) + # print(c[1]-np.cos(chi_1)*r_h) + # print(c) self.curved_path_segment(c, r_h, r_v, chi_0, chi_1, gamma_0, gamma_1) - - if i == len(waypoints)-2-1: + + if i == len(waypoints) - 2 - 1: wp_2 = self.path[-1](1) - l_2 = np.linalg.norm(v2)-tot_cut + l_2 = np.linalg.norm(v2) - tot_cut self.straight_line_path_segment(wp_2, l_2, chi_1, gamma_1) wp_0 = self.path[-1](1) def straight_line_path_segment(self, p_0, l, chi_l, gamma_l): - self.path.append(lambda varpi : [p_0[0] + l*varpi*np.cos(chi_l)*np.cos(gamma_l), - p_0[1] + l*varpi*np.sin(chi_l)*np.cos(gamma_l), - p_0[2] - l*varpi*np.sin(gamma_l)]) - self.chi_p.append(lambda varpi : chi_l) - self.gamma_p.append(lambda varpi : gamma_l) - self.kappa_h.append(lambda varpi : 0) - self.kappa_v.append(lambda varpi : 0) + self.path.append( + lambda varpi: [ + p_0[0] + l * varpi * np.cos(chi_l) * np.cos(gamma_l), + p_0[1] + l * varpi * np.sin(chi_l) * np.cos(gamma_l), + p_0[2] - l * varpi * np.sin(gamma_l), + ] + ) + self.chi_p.append(lambda varpi: chi_l) + self.gamma_p.append(lambda varpi: gamma_l) + self.kappa_h.append(lambda varpi: 0) + self.kappa_v.append(lambda varpi: 0) self.length.append(l) def curved_path_segment(self, c, r_h, r_v, chi_0, chi_1, gamma_0, gamma_1): - l_1 = abs(r_h*ssa(chi_1-chi_0)*np.sin(gamma_0))/2 - l_2 = abs(r_h*ssa(chi_1-chi_0)*np.sin(gamma_1))/2 - self.path.append(lambda varpi : [c[0] - np.sign(ssa(chi_0-chi_1))*r_h*np.sin(chi_0 + varpi*(ssa(chi_1-chi_0))), - c[1] + np.sign(ssa(chi_0-chi_1))*r_h*np.cos(chi_0 + varpi*(ssa(chi_1-chi_0))), - c[2] + (-np.sign(gamma_0)*l_1*(varpi-0.5)*2 if varpi < 0.5 else -np.sign(gamma_1)*l_2*(varpi-0.5)*2)]) - self.chi_p.append(lambda varpi : ssa(chi_0 + varpi*(ssa(chi_1-chi_0)))) - self.gamma_p.append(lambda varpi : (gamma_0 if varpi < 0.5 else gamma_1)) - self.kappa_h.append(lambda varpi : np.sign(ssa(chi_1-chi_0))*1/r_h) - self.kappa_v.append(lambda varpi : 0) - length = np.sqrt((abs(r_h*ssa(chi_1-chi_0))/2)**2 + l_1**2) + np.sqrt((abs(r_h*ssa(chi_1-chi_0))/2)**2 + l_2**2) + l_1 = abs(r_h * ssa(chi_1 - chi_0) * np.sin(gamma_0)) / 2 + l_2 = abs(r_h * ssa(chi_1 - chi_0) * np.sin(gamma_1)) / 2 + self.path.append( + lambda varpi: [ + c[0] + - np.sign(ssa(chi_0 - chi_1)) + * r_h + * np.sin(chi_0 + varpi * (ssa(chi_1 - chi_0))), + c[1] + + np.sign(ssa(chi_0 - chi_1)) + * r_h + * np.cos(chi_0 + varpi * (ssa(chi_1 - chi_0))), + c[2] + + ( + -np.sign(gamma_0) * l_1 * (varpi - 0.5) * 2 + if varpi < 0.5 + else -np.sign(gamma_1) * l_2 * (varpi - 0.5) * 2 + ), + ] + ) + self.chi_p.append(lambda varpi: ssa(chi_0 + varpi * (ssa(chi_1 - chi_0)))) + self.gamma_p.append(lambda varpi: (gamma_0 if varpi < 0.5 else gamma_1)) + self.kappa_h.append(lambda varpi: np.sign(ssa(chi_1 - chi_0)) * 1 / r_h) + self.kappa_v.append(lambda varpi: 0) + length = np.sqrt((abs(r_h * ssa(chi_1 - chi_0)) / 2) ** 2 + l_1**2) + np.sqrt( + (abs(r_h * ssa(chi_1 - chi_0)) / 2) ** 2 + l_2**2 + ) self.length.append(length) - -if __name__ == '__main__': - waypoints = [[0, 0, 0.5], [-2, 4, 2], [-4, 4, 4], [-6, 5, 4], [-8, 3, 4], [-6, 1, 4], [-4, 2, 4], [-2, 2, 2], [0, 0, 0.5]] + + +if __name__ == "__main__": + waypoints = [ + [0, 0, 0.5], + [-2, 4, 2], + [-4, 4, 4], + [-6, 5, 4], + [-8, 3, 4], + [-6, 1, 4], + [-4, 2, 4], + [-2, 2, 2], + [0, 0, 0.5], + ] path = Path() path.generate_G0_path(waypoints) - #path.generate_G1_path(waypoints, 0.2, 0.2) - + # path.generate_G1_path(waypoints, 0.2, 0.2) + data = { - 'x': [], 'y': [], 'z': [], 'gamma_p': [], 'chi_p': [], 'curvature_h': [], 'curvature_v': [], 'length': [] - } - + "x": [], + "y": [], + "z": [], + "gamma_p": [], + "chi_p": [], + "curvature_h": [], + "curvature_v": [], + "length": [], + } + for i in range(len(path.path)): for j in list(np.linspace(0, 1, 1000)): point = path.path[i](j) - data['x'].append(point[0]) - data['y'].append(point[1]) - data['z'].append(point[2]) - data['gamma_p'].append(path.gamma_p[i](j)) - data['chi_p'].append(path.chi_p[i](j)) - data['curvature_h'].append(path.kappa_h[i](j)) - data['curvature_v'].append(path.kappa_v[i](j)) - data['length'].append(path.length[i]) + data["x"].append(point[0]) + data["y"].append(point[1]) + data["z"].append(point[2]) + data["gamma_p"].append(path.gamma_p[i](j)) + data["chi_p"].append(path.chi_p[i](j)) + data["curvature_h"].append(path.kappa_h[i](j)) + data["curvature_v"].append(path.kappa_v[i](j)) + data["length"].append(path.length[i]) df = pd.DataFrame(data) - df.to_csv('matlab/path24_G0.csv') + df.to_csv("matlab/path24_G0.csv") diff --git a/motion/vtf_guidance_and_control/scripts/virtual_target.py b/motion/vtf_guidance_and_control/scripts/virtual_target.py index f4a23ea37..eb1598258 100755 --- a/motion/vtf_guidance_and_control/scripts/virtual_target.py +++ b/motion/vtf_guidance_and_control/scripts/virtual_target.py @@ -10,8 +10,21 @@ from control_allocation import ControlAllocationSystem from control_system import DPControlSystem -class VirtualTarget(): - def __init__(self, Path, AUVModel, ControlAllocationSystem, DPControlSystem, omega_b, dot_s_bounds=[-1, 1], current_velocity=[0,0,0,0,0,0], heading_mode='path_dependent_heading', point=[0, 0], speed = 0): + +class VirtualTarget: + def __init__( + self, + Path, + AUVModel, + ControlAllocationSystem, + DPControlSystem, + omega_b, + dot_s_bounds=[-1, 1], + current_velocity=[0, 0, 0, 0, 0, 0], + heading_mode="path_dependent_heading", + point=[0, 0], + speed=0, + ): self.path = Path self.auv_model = AUVModel self.control_allocation_system = ControlAllocationSystem @@ -30,77 +43,101 @@ def __init__(self, Path, AUVModel, ControlAllocationSystem, DPControlSystem, ome def update_states(self, heading_mode, point=[0, 0]): p = self.path.path[self.path_index](self.varpi) chi_p = self.path.chi_p[self.path_index](self.varpi) - dot_chi_p = lambda dot_s : kappa_h*np.cos(gamma_p)*dot_s + dot_chi_p = lambda dot_s: kappa_h * np.cos(gamma_p) * dot_s gamma_p = self.path.gamma_p[self.path_index](self.varpi) - dot_gamma_p = lambda dot_s : kappa_v * dot_s + dot_gamma_p = lambda dot_s: kappa_v * dot_s kappa_h = self.path.kappa_h[self.path_index](self.varpi) kappa_v = self.path.kappa_v[self.path_index](self.varpi) - if heading_mode == 'path_dependent_heading': + if heading_mode == "path_dependent_heading": psi_t = chi_p - dot_psi_t = lambda dot_s : kappa_h*np.cos(gamma_p)*dot_s - if heading_mode == 'point_dependent_heading': - kappa_o = 1/np.linalg.norm([point[0] - p[0], point[1] - p[1]]) - dot_kappa_o = lambda dot_s : 1/(np.cos(chi_p - psi_t)*np.cos(gamma_p)*dot_s) + dot_psi_t = lambda dot_s: kappa_h * np.cos(gamma_p) * dot_s + if heading_mode == "point_dependent_heading": + kappa_o = 1 / np.linalg.norm([point[0] - p[0], point[1] - p[1]]) + dot_kappa_o = lambda dot_s: 1 / ( + np.cos(chi_p - psi_t) * np.cos(gamma_p) * dot_s + ) psi_t = np.arctan2(point[1] - p[1], point[0] - p[0]) - dot_psi_t = lambda dot_s : -kappa_o*np.sin(chi_p - psi_t)*dot_s + dot_psi_t = lambda dot_s: -kappa_o * np.sin(chi_p - psi_t) * dot_s self.eta_t = np.array(p + [0.0, 0.0, psi_t]) - self.nu_t = lambda dot_s : np.array([np.cos(chi_p - psi_t)*np.cos(gamma_p)*dot_s] + - [np.sin(chi_p - psi_t)*np.cos(gamma_p)*dot_s] + - [-np.sin(gamma_p)*dot_s] + - [0] + - [0] + - [dot_psi_t(dot_s)]) + self.nu_t = lambda dot_s: np.array( + [np.cos(chi_p - psi_t) * np.cos(gamma_p) * dot_s] + + [np.sin(chi_p - psi_t) * np.cos(gamma_p) * dot_s] + + [-np.sin(gamma_p) * dot_s] + + [0] + + [0] + + [dot_psi_t(dot_s)] + ) def optimize_along_track_speed(self, eta, nu, dot_eta_c, t): - if not hasattr(self, 't'): + if not hasattr(self, "t"): self.t = t h = t - self.t - tau_c = lambda dot_s : self.dp_control_system.pd_regulate(eta, nu, self.eta_t, self.nu_t(dot_s), dot_eta_c) - self.auv_model.gvect #+ np.dot(self.auv_model.D, (self.nu_t(dot_s)-nu_c)) - np.dot(self.auv_model.M, dot_nu_c) - f = lambda x : -x - g_1 = lambda x : -(np.dot(self.control_allocation_system.pseudo_inv_input_matrix, tau_c(x)) - self.control_allocation_system.u_max) - g_2 = lambda x : -(-np.dot(self.control_allocation_system.pseudo_inv_input_matrix, tau_c(x)) + self.control_allocation_system.u_min) + tau_c = ( + lambda dot_s: self.dp_control_system.pd_regulate( + eta, nu, self.eta_t, self.nu_t(dot_s), dot_eta_c + ) + - self.auv_model.gvect + ) # + np.dot(self.auv_model.D, (self.nu_t(dot_s)-nu_c)) - np.dot(self.auv_model.M, dot_nu_c) + f = lambda x: -x + g_1 = lambda x: -( + np.dot(self.control_allocation_system.pseudo_inv_input_matrix, tau_c(x)) + - self.control_allocation_system.u_max + ) + g_2 = lambda x: -( + -np.dot(self.control_allocation_system.pseudo_inv_input_matrix, tau_c(x)) + + self.control_allocation_system.u_min + ) x_0 = self.dot_s bnds = [self.dot_s_bounds] - con1 = {'type': 'ineq', 'fun': g_1} - con2 = {'type': 'ineq', 'fun': g_2} + con1 = {"type": "ineq", "fun": g_1} + con2 = {"type": "ineq", "fun": g_2} cons = [con1, con2] - options = {'maxiter': 5, 'ftol': 0.01, 'eps': 0.0001} - sol = minimize(f, x_0, method='SLSQP', bounds=bnds, constraints=cons, options=options) + options = {"maxiter": 5, "ftol": 0.01, "eps": 0.0001} + sol = minimize( + f, x_0, method="SLSQP", bounds=bnds, constraints=cons, options=options + ) if sol.success: dot_s_d = sol.x[0] else: dot_s_d = 0 - ddot_s = (dot_s_d - self.dot_s)/2.5 + ddot_s = (dot_s_d - self.dot_s) / 2.5 self.dot_s = euler2(ddot_s, self.dot_s, h) - + def simulate_path_variables(self, t): - if not hasattr(self, 't'): + if not hasattr(self, "t"): self.t = t h = t - self.t - dot_varpi = self.dot_s/self.path.length[self.path_index] + dot_varpi = self.dot_s / self.path.length[self.path_index] self.varpi = euler2(dot_varpi, self.varpi, h) self.t = t if self.varpi > 1: - if not self.path_index == len(self.path.path)-1: + if not self.path_index == len(self.path.path) - 1: self.path_index += 1 rest = self.varpi - 1 - self.varpi = rest * self.path.length[self.path_index-1]/self.path.length[self.path_index] + self.varpi = ( + rest + * self.path.length[self.path_index - 1] + / self.path.length[self.path_index] + ) else: self.varpi = 1 self.dot_s = 0 self.s = 0 for i in range(self.path_index): self.s += self.path.length[i] - self.s += self.path.length[self.path_index]*self.varpi + self.s += self.path.length[self.path_index] * self.varpi - def generate_reference_trajectories(self, eta, nu, t, heading_mode, point=[0, 0], dot_eta_c=[0,0,0,0,0,0]): + def generate_reference_trajectories( + self, eta, nu, t, heading_mode, point=[0, 0], dot_eta_c=[0, 0, 0, 0, 0, 0] + ): self.update_states(heading_mode, point=point) self.optimize_along_track_speed(eta, nu, dot_eta_c, t) self.simulate_path_variables(t) - return self.eta_t, self.nu_t(self.dot_s),self.dot_s + return self.eta_t, self.nu_t(self.dot_s), self.dot_s -if __name__ == '__main__': + +if __name__ == "__main__": waypoints = [[0, 0, 0], [1, 0, 1], [0, 1, 0], [0, 0, 0]] path = Path() path.generate_G0_path(waypoints) @@ -108,30 +145,49 @@ def generate_reference_trajectories(self, eta, nu, t, heading_mode, point=[0, 0] m = 30.9 r_g = [0, 0, 0.02] r_b = [0, 0, -0.05] - inertia = [[ 0.503217, 0.000204, -0.000526], - [ 0.000204, 0.893449, 0.000038], - [ -0.000526, 0.000038, 0.919819]] + inertia = [ + [0.503217, 0.000204, -0.000526], + [0.000204, 0.893449, 0.000038], + [-0.000526, 0.000038, 0.919819], + ] volume = 0.0295 - M_A = [[ 10.7727, 0, 0, 0, 0, 0], - [ 0, 10.7727, 0, 0, 0, 0], - [ 0, 0, 49.7679, 0, 0, 0], - [ 0, 0, 0, 1.0092, 0, 0], - [ 0, 0, 0, 0, 1.0092, 0], - [ 0, 0, 0, 0, 0, 0]] - D = [[ -9.5909, 0, 0, 0, 0, 0], - [ 0, -9.5909, 0, 0, 0, 0], - [ 0, 0, -50.5595, 0, 0, 0], - [ 0, 0, 0, -13.3040, 0, 0], - [ 0, 0, 0, 0, -13.3040, 0], - [ 0, 0, 0, 0, 0, -5.1559]] - auv_model = AUVModel(m, r_g, r_b, inertia, volume, M_A, D, rho=997, g=9.81, dot_eta_c=[0,0,0,0,0,0]) - - input_matrix = [[-0.707, 0.000, 0.000, 0.707, 0.707, 0.000, 0.000, -0.707], - [-0.707, 0.000, 0.000, -0.707, 0.707, 0.000, 0.000, 0.707], - [ 0.000, 1.000, 1.000, 0.000, 0.000, 1.000, 1.000, 0.000], - [ 0.046, -0.220, -0.220, 0.046, -0.046, 0.220, 0.220, -0.046], - [-0.046, -0.120, 0.120, 0.046, 0.046, 0.119, -0.119, -0.046], - [-0.324, 0.000, 0.000, 0.325, -0.325, 0.000, 0.000, 0.324]] + M_A = [ + [10.7727, 0, 0, 0, 0, 0], + [0, 10.7727, 0, 0, 0, 0], + [0, 0, 49.7679, 0, 0, 0], + [0, 0, 0, 1.0092, 0, 0], + [0, 0, 0, 0, 1.0092, 0], + [0, 0, 0, 0, 0, 0], + ] + D = [ + [-9.5909, 0, 0, 0, 0, 0], + [0, -9.5909, 0, 0, 0, 0], + [0, 0, -50.5595, 0, 0, 0], + [0, 0, 0, -13.3040, 0, 0], + [0, 0, 0, 0, -13.3040, 0], + [0, 0, 0, 0, 0, -5.1559], + ] + auv_model = AUVModel( + m, + r_g, + r_b, + inertia, + volume, + M_A, + D, + rho=997, + g=9.81, + dot_eta_c=[0, 0, 0, 0, 0, 0], + ) + + input_matrix = [ + [-0.707, 0.000, 0.000, 0.707, 0.707, 0.000, 0.000, -0.707], + [-0.707, 0.000, 0.000, -0.707, 0.707, 0.000, 0.000, 0.707], + [0.000, 1.000, 1.000, 0.000, 0.000, 1.000, 1.000, 0.000], + [0.046, -0.220, -0.220, 0.046, -0.046, 0.220, 0.220, -0.046], + [-0.046, -0.120, 0.120, 0.046, 0.046, 0.119, -0.119, -0.046], + [-0.324, 0.000, 0.000, 0.325, -0.325, 0.000, 0.000, 0.324], + ] rotor_time_constant = 0.2 u_max = 31.5 u_min = -31.5 @@ -143,19 +199,19 @@ def generate_reference_trajectories(self, eta, nu, t, heading_mode, point=[0, 0] virtual_target = VirtualTarget(path, auv_model, actuator_model, dp_control_system) - eta = [1,0,0,0,0,0] - nu = [0.1,0,0,0,0,0] - dot_eta_c = [0,0,0,0,0,0] + eta = [1, 0, 0, 0, 0, 0] + nu = [0.1, 0, 0, 0, 0, 0] + dot_eta_c = [0, 0, 0, 0, 0, 0] virtual_target.optimize_along_track_speed(eta, nu, dot_eta_c) h = 0.01 N = 100 t = 0 for i in range(N): - t = i*h - eta_r, nu_r, dot_nu_r = virtual_target.generate_reference_trajectories(eta, nu, t) + t = i * h + eta_r, nu_r, dot_nu_r = virtual_target.generate_reference_trajectories( + eta, nu, t + ) eta = eta_r nu = nu_r print(eta_r[1]) - - \ No newline at end of file diff --git a/motion/vtf_guidance_and_control/scripts/vtf_server.py b/motion/vtf_guidance_and_control/scripts/vtf_server.py index 9536a61ef..8fb82b5ec 100755 --- a/motion/vtf_guidance_and_control/scripts/vtf_server.py +++ b/motion/vtf_guidance_and_control/scripts/vtf_server.py @@ -7,122 +7,135 @@ # action message import actionlib -from vortex_msgs.msg import VtfPathFollowingAction, VtfPathFollowingGoal, VtfPathFollowingResult +from vortex_msgs.msg import ( + VtfPathFollowingAction, + VtfPathFollowingGoal, + VtfPathFollowingResult, +) from dynamic_reconfigure.server import Server from vtf_guidance_and_control.cfg import vtf_controllerConfig class VtfPathFollowing(object): - """ - This is the wrapper class for the VtfGuidanceAndControl class. - - Attributes: - _result A vortex_msgs action, true if a goal is set within the - sphereof acceptance, false if not - - Nodes created: - vtf_server - - Subscribes to: - /odometry/filtered - - """ - - # create messages that are used to send feedback/result - _result = VtfPathFollowingResult() - - def __init__(self): - """ - To initialize the ROS wrapper, the node, subscribers - are set up, as well as the action server. - """ - - rospy.init_node('vtf_server') - while rospy.get_time() == 0: - continue - - '''Flag to indicate if a vtf guidance is active or not''' - self.publish_guidance_data = False - - # parameters - rate = rospy.get_param("/guidance/vtf/rate", default=20) - self.ros_rate = rospy.Rate(rate) - - # constructor object - self.vtf = VtfGuidanceAndControlNode() - - # Action server, see https://github.com/strawlab/ros_common/blob/master/actionlib/src/actionlib/simple_action_server.py - self.action_server = actionlib.SimpleActionServer(name='vtf_action_server', ActionSpec=VtfPathFollowingAction, auto_start=False) - self.action_server.register_goal_callback(self.goal_cb) - self.action_server.start() - self.vtf_reconfigure_srv = Server(vtf_controllerConfig, self.vtf.vtf_reconfigure) - - - rospy.loginfo("vtf guidance initiated") - - def spin(self): - while not rospy.is_shutdown(): - try: - if self.publish_guidance_data: - self.vtf.publish_control_forces() - self.statusActionGoal() - self.ros_rate.sleep() - except rospy.ROSInterruptException: - pass - - - - def statusActionGoal(self): - """ - Checks if a preempt request has been set or if the goal was reached. - For both cases it publishes a 0 wrench to the thruster manager - and sets the action server to preempted or succeeded. - """ - if self.action_server.is_preempt_requested(): - rospy.loginfo("Preempted requested by vtf path client") - self.publish_guidance_data = False - msg = create_wrench_msg([0,0,0,0,0,0]) - self.vtf.pub.publish(msg) - self.action_server.set_preempted() - - # succeeded - if self.vtf.goal_reached: - self._result.terminalSector = True - self.publish_guidance_data = False - self.vtf.goal_reached = False - self.action_server.set_succeeded(self._result, text="goal completed") - msg = create_wrench_msg([0,0,0,0,0,0]) - self.vtf.pub.publish(msg) - - - def goal_cb(self): - """ - The goal callback for the action server. - - Once a goal has been recieved from the client, self.publish_guidance_data is set to True - """ - - _goal = self.action_server.accept_new_goal() - rospy.logdebug("vtf_guidance recieved new goal") - - #reset goal reached - self.vtf.goal_reached = False - # set goal, the first item will be replaced by current position by the vtf controller so init with dummy: - self.vtf.waypoints = [[6,6,6]] - for wp in _goal.waypoints: - self.vtf.waypoints.append([wp.x,wp.y, wp.z]) - if self.publish_guidance_data: - self.vtf.update_path(_goal.forward_speed,_goal.heading, [_goal.heading_point.x,_goal.heading_point.y]) - else: - self.vtf.new_path_recieved(_goal.forward_speed,_goal.heading, [_goal.heading_point.x,_goal.heading_point.y]) - self.publish_guidance_data = True - - - -if __name__ == '__main__': - try: - vtf_path_following = VtfPathFollowing() - vtf_path_following.spin() - - except rospy.ROSInterruptException: - pass + """ + This is the wrapper class for the VtfGuidanceAndControl class. + + Attributes: + _result A vortex_msgs action, true if a goal is set within the + sphereof acceptance, false if not + + Nodes created: + vtf_server + + Subscribes to: + /odometry/filtered + + """ + + # create messages that are used to send feedback/result + _result = VtfPathFollowingResult() + + def __init__(self): + """ + To initialize the ROS wrapper, the node, subscribers + are set up, as well as the action server. + """ + + rospy.init_node("vtf_server") + while rospy.get_time() == 0: + continue + + """Flag to indicate if a vtf guidance is active or not""" + self.publish_guidance_data = False + + # parameters + rate = rospy.get_param("/guidance/vtf/rate", default=20) + self.ros_rate = rospy.Rate(rate) + + # constructor object + self.vtf = VtfGuidanceAndControlNode() + + # Action server, see https://github.com/strawlab/ros_common/blob/master/actionlib/src/actionlib/simple_action_server.py + self.action_server = actionlib.SimpleActionServer( + name="vtf_action_server", + ActionSpec=VtfPathFollowingAction, + auto_start=False, + ) + self.action_server.register_goal_callback(self.goal_cb) + self.action_server.start() + self.vtf_reconfigure_srv = Server( + vtf_controllerConfig, self.vtf.vtf_reconfigure + ) + + rospy.loginfo("vtf guidance initiated") + + def spin(self): + while not rospy.is_shutdown(): + try: + if self.publish_guidance_data: + self.vtf.publish_control_forces() + self.statusActionGoal() + self.ros_rate.sleep() + except rospy.ROSInterruptException: + pass + + def statusActionGoal(self): + """ + Checks if a preempt request has been set or if the goal was reached. + For both cases it publishes a 0 wrench to the thruster manager + and sets the action server to preempted or succeeded. + """ + if self.action_server.is_preempt_requested(): + rospy.loginfo("Preempted requested by vtf path client") + self.publish_guidance_data = False + msg = create_wrench_msg([0, 0, 0, 0, 0, 0]) + self.vtf.pub.publish(msg) + self.action_server.set_preempted() + + # succeeded + if self.vtf.goal_reached: + self._result.terminalSector = True + self.publish_guidance_data = False + self.vtf.goal_reached = False + self.action_server.set_succeeded(self._result, text="goal completed") + msg = create_wrench_msg([0, 0, 0, 0, 0, 0]) + self.vtf.pub.publish(msg) + + def goal_cb(self): + """ + The goal callback for the action server. + + Once a goal has been recieved from the client, self.publish_guidance_data is set to True + """ + + _goal = self.action_server.accept_new_goal() + rospy.logdebug("vtf_guidance recieved new goal") + + # reset goal reached + self.vtf.goal_reached = False + # set goal, the first item will be replaced by current position by the vtf controller so init with dummy: + self.vtf.waypoints = [[6, 6, 6]] + for wp in _goal.waypoints: + self.vtf.waypoints.append([wp.x, wp.y, wp.z]) + if self.publish_guidance_data: + self.vtf.update_path( + _goal.forward_speed, + _goal.heading, + [_goal.heading_point.x, _goal.heading_point.y], + ) + else: + self.vtf.new_path_recieved( + _goal.forward_speed, + _goal.heading, + [_goal.heading_point.x, _goal.heading_point.y], + ) + self.publish_guidance_data = True + + +if __name__ == "__main__": + try: + vtf_path_following = VtfPathFollowing() + vtf_path_following.spin() + + except rospy.ROSInterruptException: + pass diff --git a/navigation/ESKF/apps/ros/ros_node.cpp b/navigation/ESKF/apps/ros/ros_node.cpp old mode 100755 new mode 100644 index cbd082317..6236a8366 --- a/navigation/ESKF/apps/ros/ros_node.cpp +++ b/navigation/ESKF/apps/ros/ros_node.cpp @@ -166,7 +166,7 @@ void ESKF_Node::dvlCallback(const geometry_msgs::TwistWithCovarianceStamped::Con // Publish DVL - NIS nav_msgs::Odometry odom_msg; static size_t trace_id{ 0 }; - odom_msg.header.frame_id = "/odom";// "/eskf_link"; + odom_msg.header.frame_id = "/odom"; // "/eskf_link"; odom_msg.header.seq = trace_id++; odom_msg.header.stamp = ros::Time::now(); odom_msg.pose.pose.position.x = eskf_.getNISDVL(); @@ -176,9 +176,8 @@ void ESKF_Node::dvlCallback(const geometry_msgs::TwistWithCovarianceStamped::Con // PressureZ subscriber void ESKF_Node::pressureZCallback(const std_msgs::Float64::ConstPtr& depth_msg) { - double mbar = depth_msg->data / 10.0; - double depth_mm = mbar * 10.197; // 10.197 is the constant to convert mbar to mmH2O + double depth_mm = mbar * 10.197; // 10.197 is the constant to convert mbar to mmH2O TAC_z = depth_mm / 1000.0; Matrix RpressureZ; const double raw_pressure_z = depth_msg->data; @@ -208,7 +207,7 @@ void ESKF_Node::pressureZCallback(const std_msgs::Float64::ConstPtr& depth_msg) // Publish Pressure Z - NIS nav_msgs::Odometry odom_msg; static size_t trace_id{ 0 }; - odom_msg.header.frame_id = "/odom";//"/eskf_link"; + odom_msg.header.frame_id = "/odom"; //"/eskf_link"; odom_msg.header.seq = trace_id++; odom_msg.header.stamp = ros::Time::now(); odom_msg.pose.pose.position.x = eskf_.getNISPressureZ(); @@ -243,7 +242,7 @@ void ESKF_Node::publishPoseState(const ros::TimerEvent&) attitude_error_covariance.setZero(); attitude_error_covariance = errorCovariance.block<3, 3>(6, 6); - imu_biases_and_gravity.header.frame_id = "/odom"; //"/eskf_link"; + imu_biases_and_gravity.header.frame_id = "/odom"; //"/eskf_link"; imu_biases_and_gravity.header.seq = trace_id++; imu_biases_and_gravity.header.stamp = ros::Time::now(); imu_biases_and_gravity.pose.pose.position.x = accbias(0); @@ -258,21 +257,21 @@ void ESKF_Node::publishPoseState(const ros::TimerEvent&) publishAccGyrobiasandGravity_.publish(imu_biases_and_gravity); - odom_msg.header.frame_id = "/odom"; // "/eskf_link"; + odom_msg.header.frame_id = "/odom"; // "/eskf_link"; odom_msg.header.seq = trace_id++; odom_msg.header.stamp = ros::Time::now(); odom_msg.pose.pose.position.x = position(StateMemberX); // NEDpose(StateMemberX); odom_msg.pose.pose.position.y = position(StateMemberY); // NEDpose(StateMemberY) - //odom_msg.pose.pose.position.z = position(StateMemberZ); // NEDpose(StateMemberZ); + // odom_msg.pose.pose.position.z = position(StateMemberZ); // NEDpose(StateMemberZ); odom_msg.pose.pose.position.z = TAC_z; - odom_msg.twist.twist.linear.x = velocity(0); // NEDpose(StateMemberVx); - odom_msg.twist.twist.linear.y = velocity(1); // NEDpose(StateMemberVy); - odom_msg.twist.twist.linear.z = velocity(2); // NEDpose(StateMemberVz); + odom_msg.twist.twist.linear.x = velocity(0); // NEDpose(StateMemberVx); + odom_msg.twist.twist.linear.y = velocity(1); // NEDpose(StateMemberVy); + odom_msg.twist.twist.linear.z = velocity(2); // NEDpose(StateMemberVz); odom_msg.twist.twist.angular = angular_vel; - odom_msg.pose.pose.orientation.w = quaternion.w(); // pose(StateMemberQw); - odom_msg.pose.pose.orientation.x = quaternion.x(); // pose(StateMemberQx); - odom_msg.pose.pose.orientation.y = quaternion.y(); // pose(StateMemberQy); - odom_msg.pose.pose.orientation.z = quaternion.z(); // pose(StateMemberQz); + odom_msg.pose.pose.orientation.w = quaternion.w(); // pose(StateMemberQw); + odom_msg.pose.pose.orientation.x = quaternion.x(); // pose(StateMemberQx); + odom_msg.pose.pose.orientation.y = quaternion.y(); // pose(StateMemberQy); + odom_msg.pose.pose.orientation.z = quaternion.z(); // pose(StateMemberQz); // odom_msg.pose.covariance diff --git a/navigation/ESKF/apps/test/main.cpp b/navigation/ESKF/apps/test/main.cpp old mode 100755 new mode 100644 index 4291b1348..845926841 --- a/navigation/ESKF/apps/test/main.cpp +++ b/navigation/ESKF/apps/test/main.cpp @@ -5,16 +5,15 @@ using namespace Eigen; int main(int argc, char *argv[]) { - /* - ros::init(argc,argv,"eskf"); - ros::NodeHandle nh; - ros::NodeHandle pnh("~"); - ESKF_Node eskf_node(nh,pnh); - ros::spin(); - return 0; - */ - - + /* + ros::init(argc,argv,"eskf"); + ros::NodeHandle nh; + ros::NodeHandle pnh("~"); + ESKF_Node eskf_node(nh,pnh); + ros::spin(); + return 0; + */ + // Matrix3d test = Matrix3d::Zero(); // double* arraytesting= test.data(); @@ -92,30 +91,29 @@ int main(int argc, char *argv[]) } auto end = std::chrono::steady_clock::now(); auto diff = end - start; - std::cout << "predictnominal: " << std::chrono::duration(diff).count() << " ms" - << std::endl; + std::cout << "predictnominal: " << std::chrono::duration(diff).count() << " ms" << std::endl; - // Execution time - //auto end = std::chrono::steady_clock::now(); + // Execution time + // auto end = std::chrono::steady_clock::now(); - //auto diff = end - start; + // auto diff = end - start; - //auto diff_in_ms = std::chrono::duration (diff).count(); + // auto diff_in_ms = std::chrono::duration (diff).count(); - //std::cout< #include - - namespace eskf { enum NominalStateMembers //(16x1) @@ -59,7 +57,9 @@ struct AdandGQGD struct StatesAndErrorCovariance { Eigen::Matrix X{ Eigen::Matrix::Zero() }; - Eigen::Matrix P{ Eigen::Matrix::Zero() }; + Eigen::Matrix P{ + Eigen::Matrix::Zero() + }; }; struct InnovationPressureStates @@ -124,62 +124,65 @@ struct parametersInESKF bool use_ENU; }; - struct StateAndCovariance_msg { double timeStamp_; Eigen::Matrix X_; Eigen::Matrix P_; - StateAndCovariance_msg() - :timeStamp_{0} + StateAndCovariance_msg() : timeStamp_{ 0 } { X_.setZero(); P_.setZero(); } - StateAndCovariance_msg(const Eigen::Matrix& XState, const Eigen::Matrix& PState,const double& timeStamp) - :X_{XState},P_{PState},timeStamp_{timeStamp} + StateAndCovariance_msg(const Eigen::Matrix& XState, + const Eigen::Matrix& PState, + const double& timeStamp) + : X_{ XState }, P_{ PState }, timeStamp_{ timeStamp } { } - }; -struct IMUmessage +struct IMUmessage { bool predicted_msg_; double timeStamp_; double deltaIMU_; Eigen::Vector3d zAccMeasurement_; Eigen::Vector3d zGyroMeasurement_; - Eigen::Matrix R_acc_; - Eigen::Matrix R_gyro_; - IMUmessage() - :timeStamp_{0},deltaIMU_{0},predicted_msg_{false} + Eigen::Matrix R_acc_; + Eigen::Matrix R_gyro_; + IMUmessage() : timeStamp_{ 0 }, deltaIMU_{ 0 }, predicted_msg_{ false } { zAccMeasurement_.setZero(); zGyroMeasurement_.setZero(); R_acc_.setZero(); R_gyro_.setZero(); } - IMUmessage(const double& timeStamp,const double& deltaIMU, const Eigen::Vector3d& zAcc, const Eigen::Vector3d& zGyro, const Eigen::Matrix& Racc, const Eigen::Matrix Rgyro) - : timeStamp_{timeStamp},deltaIMU_{deltaIMU},zAccMeasurement_{zAcc},zGyroMeasurement_{zGyro},R_acc_{Racc},R_gyro_{Rgyro},predicted_msg_{false} + IMUmessage(const double& timeStamp, const double& deltaIMU, const Eigen::Vector3d& zAcc, const Eigen::Vector3d& zGyro, + const Eigen::Matrix& Racc, const Eigen::Matrix Rgyro) + : timeStamp_{ timeStamp } + , deltaIMU_{ deltaIMU } + , zAccMeasurement_{ zAcc } + , zGyroMeasurement_{ zGyro } + , R_acc_{ Racc } + , R_gyro_{ Rgyro } + , predicted_msg_{ false } { } - }; struct DVLmessage { double timeStamp_; Eigen::Vector3d zDVl_; - Eigen::Matrix R_dvl_; - DVLmessage() - :timeStamp_{0} + Eigen::Matrix R_dvl_; + DVLmessage() : timeStamp_{ 0 } { zDVl_.setZero(); R_dvl_.setZero(); } - DVLmessage(const double& timeStamp, const Eigen::Vector3d zDvl, const Eigen::Matrix R_dvl) - :timeStamp_{timeStamp},zDVl_{zDvl},R_dvl_{R_dvl} + DVLmessage(const double& timeStamp, const Eigen::Vector3d zDvl, const Eigen::Matrix R_dvl) + : timeStamp_{ timeStamp }, zDVl_{ zDvl }, R_dvl_{ R_dvl } { } }; @@ -188,14 +191,13 @@ struct PressureZmessage { double timeStamp_; double pressureZ_msg_; - Eigen::Matrix R_pressureZ_; - PressureZmessage() - :timeStamp_{0}, pressureZ_msg_{0} + Eigen::Matrix R_pressureZ_; + PressureZmessage() : timeStamp_{ 0 }, pressureZ_msg_{ 0 } { R_pressureZ_.setZero(); } - PressureZmessage(const double& timeStamp, const double& pressureZ_msg, Eigen::Matrix R_pressureZ) - :timeStamp_{timeStamp},pressureZ_msg_{pressureZ_msg},R_pressureZ_{R_pressureZ} + PressureZmessage(const double& timeStamp, const double& pressureZ_msg, Eigen::Matrix R_pressureZ) + : timeStamp_{ timeStamp }, pressureZ_msg_{ pressureZ_msg }, R_pressureZ_{ R_pressureZ } { } }; @@ -205,20 +207,24 @@ class ESKF public: explicit ESKF(const parametersInESKF& parameters); - explicit ESKF(Eigen::Matrix3d Racc, Eigen::Matrix3d RaccBias, Eigen::Matrix3d Rgyro, Eigen::Matrix3d RgyroBias, double pgyroBias, double paccBias, - Eigen::Matrix3d Sa, Eigen::Matrix3d Sg, Eigen::Matrix3d Sdvl, Eigen::Matrix3d Sinc); + explicit ESKF(Eigen::Matrix3d Racc, Eigen::Matrix3d RaccBias, Eigen::Matrix3d Rgyro, Eigen::Matrix3d RgyroBias, + double pgyroBias, double paccBias, Eigen::Matrix3d Sa, Eigen::Matrix3d Sg, Eigen::Matrix3d Sdvl, + Eigen::Matrix3d Sinc); void predictWithBuffer(); void predict(const Eigen::Vector3d& zAccMeasurements, const Eigen::Vector3d& zGyroMeasurements, const double& Ts, - const Eigen::Matrix3d& Racc, const Eigen::Matrix3d& Rgyro); + const Eigen::Matrix3d& Racc, const Eigen::Matrix3d& Rgyro); void updateDVL(const Eigen::Vector3d& zDVLvel, const Eigen::Matrix3d& RDVL); void updatePressureZ(const double& zPressureZpos, const Eigen::MatrixXd& RpressureZ); - void bufferIMUMessages(const Eigen::Vector3d& zAccMeasurements, const Eigen::Vector3d& zGyroMeasurements, const double& timeStamp,const double& deltaIMU, const Eigen::Matrix3d& Racc, const Eigen::Matrix3d& Rgyro); - void bufferDVLMessages(const Eigen::Vector3d& zDvlMeasurements,const double timeStamp,const Eigen::Matrix3d& Rdvl); - void bufferPressureZMessages(const double& pressureZ,const double& timeStamp, Eigen::Matrix R_pressureZ); + void bufferIMUMessages(const Eigen::Vector3d& zAccMeasurements, const Eigen::Vector3d& zGyroMeasurements, + const double& timeStamp, const double& deltaIMU, const Eigen::Matrix3d& Racc, + const Eigen::Matrix3d& Rgyro); + void bufferDVLMessages(const Eigen::Vector3d& zDvlMeasurements, const double timeStamp, const Eigen::Matrix3d& Rdvl); + void bufferPressureZMessages(const double& pressureZ, const double& timeStamp, + Eigen::Matrix R_pressureZ); void update(); void UpdateOnlyWithPrediction(); - //void updateDVL(const Eigen::Vector3d& zDVLvel, const Eigen::Matrix3d& RDVL); - //void updatePressureZ(const double& zPressureZpos, const Eigen::MatrixXd& RpressureZ); + // void updateDVL(const Eigen::Vector3d& zDVLvel, const Eigen::Matrix3d& RDVL); + // void updatePressureZ(const double& zPressureZpos, const Eigen::MatrixXd& RpressureZ); void updateDVLWithBuffer(); void updatePressureZWithBuffer(); @@ -228,29 +234,27 @@ class ESKF if (use_ENU_) { return Eigen::Quaterniond{ optimizationParameters_.X(StateMemberQw), optimizationParameters_.X(StateMemberQy), - optimizationParameters_.X(StateMemberQx), -optimizationParameters_.X(StateMemberQz) }; + optimizationParameters_.X(StateMemberQx), -optimizationParameters_.X(StateMemberQz) }; } else // use NED { return Eigen::Quaterniond{ optimizationParameters_.X(StateMemberQw), optimizationParameters_.X(StateMemberQx), - optimizationParameters_.X(StateMemberQy), optimizationParameters_.X(StateMemberQz) }; + optimizationParameters_.X(StateMemberQy), optimizationParameters_.X(StateMemberQz) }; } } inline void setPositionAndQuaternion(Eigen::Vector3d position, Eigen::Vector4d quat) { - optimizationParameters_.X.block(NOMINAL_POSITION_STATE_OFFSET, 0) = position; - optimizationParameters_.X.block(NOMINAL_QUATERNION_STATE_OFFSET,0) = quat; + optimizationParameters_.X.block(NOMINAL_QUATERNION_STATE_OFFSET, 0) = quat; } - inline Eigen::Vector3d getPosition() const { Eigen::Matrix3d R_ned_to_enu = Eigen::Matrix3d::Zero(); Eigen::Vector3d position = Eigen::Vector3d::Zero(); - R_ned_to_enu << 1, 0, 0, 0, -1, 0, 0, 0, 1; // TODO: Add correct transformation with EKF + R_ned_to_enu << 1, 0, 0, 0, -1, 0, 0, 0, 1; // TODO: Add correct transformation with EKF position = optimizationParameters_.X.block(NOMINAL_POSITION_STATE_OFFSET, 0); @@ -301,15 +305,15 @@ class ESKF inline Eigen::Vector3d getGyroBias() const { - return optimizationParameters_.X.block(NOMINAL_GYRO_BIAS_STATE_OFFSET,0); + return optimizationParameters_.X.block(NOMINAL_GYRO_BIAS_STATE_OFFSET, 0); } inline Eigen::Vector3d getAccBias() const { - return optimizationParameters_.X.block(NOMINAL_ACC_BIAS_STATE_OFFSET,0); + return optimizationParameters_.X.block(NOMINAL_ACC_BIAS_STATE_OFFSET, 0); } - inline double getNISPressureZ() const + inline double getNISPressureZ() const { return NISPressureZ_; } @@ -321,34 +325,39 @@ class ESKF private: Eigen::VectorXd predictNominal(const Eigen::VectorXd& xnominal, const Eigen::Vector3d& accRectifiedMeasurements, - const Eigen::Vector3d& gyroRectifiedmeasurements, const double& Ts) const; + const Eigen::Vector3d& gyroRectifiedmeasurements, const double& Ts) const; - Eigen::MatrixXd predictCovariance(const Eigen::VectorXd& xnominal, const Eigen::MatrixXd& P, const Eigen::Vector3d& accRectifiedMeasurements, - const Eigen::Vector3d& gyroRectifiedmeasurements, const double& Ts, const Eigen::Matrix3d& Racc, - const Eigen::Matrix3d& Rgyro) const; + Eigen::MatrixXd predictCovariance(const Eigen::VectorXd& xnominal, const Eigen::MatrixXd& P, + const Eigen::Vector3d& accRectifiedMeasurements, + const Eigen::Vector3d& gyroRectifiedmeasurements, const double& Ts, + const Eigen::Matrix3d& Racc, const Eigen::Matrix3d& Rgyro) const; AdandGQGD discreteErrorMatrix(const Eigen::VectorXd& xnominal, const Eigen::Vector3d& accRectifiedMeasurements, - const Eigen::Vector3d& gyroRectifiedmeasurements, const double& Ts, const Eigen::Matrix3d& Racc, - const Eigen::Matrix3d& Rgyro) const; + const Eigen::Vector3d& gyroRectifiedmeasurements, const double& Ts, + const Eigen::Matrix3d& Racc, const Eigen::Matrix3d& Rgyro) const; // Lower computation time implementation - Eigen::MatrixXd AerrDiscretizedFirstOrder(const Eigen::VectorXd& xnominal, const Eigen::Vector3d& accRectifiedMeasurements, - const Eigen::Vector3d& gyroRectifiedmeasurements, const double& Ts) const; - Eigen::MatrixXd AerrDiscretizedSecondOrder(const Eigen::VectorXd& xnominal, const Eigen::Vector3d& accRectifiedMeasurements, - const Eigen::Vector3d& gyroRectifiedmeasurements, const double& Ts) const; - Eigen::MatrixXd AerrDiscretizedThirdOrder(const Eigen::VectorXd& xnominal, const Eigen::Vector3d& accRectifiedMeasurements, - const Eigen::Vector3d& gyroRectifiedmeasurements, const double& Ts) const; + Eigen::MatrixXd AerrDiscretizedFirstOrder(const Eigen::VectorXd& xnominal, + const Eigen::Vector3d& accRectifiedMeasurements, + const Eigen::Vector3d& gyroRectifiedmeasurements, const double& Ts) const; + Eigen::MatrixXd AerrDiscretizedSecondOrder(const Eigen::VectorXd& xnominal, + const Eigen::Vector3d& accRectifiedMeasurements, + const Eigen::Vector3d& gyroRectifiedmeasurements, const double& Ts) const; + Eigen::MatrixXd AerrDiscretizedThirdOrder(const Eigen::VectorXd& xnominal, + const Eigen::Vector3d& accRectifiedMeasurements, + const Eigen::Vector3d& gyroRectifiedmeasurements, const double& Ts) const; Eigen::Matrix3d AngularErrorMatrix(const Eigen::Vector3d& gyroRectifiedmeasurements, const double& Ts) const; Eigen::MatrixXd Aerr(const Eigen::VectorXd& xnominal, const Eigen::Vector3d& accRectifiedMeasurements, - const Eigen::Vector3d& gyroRectifiedmeasurements) const; + const Eigen::Vector3d& gyroRectifiedmeasurements) const; - StatesAndErrorCovariance inject(const Eigen::VectorXd& xnominal, const Eigen::VectorXd& deltaX, const Eigen::MatrixXd& P) const; + StatesAndErrorCovariance inject(const Eigen::VectorXd& xnominal, const Eigen::VectorXd& deltaX, + const Eigen::MatrixXd& P) const; - InnovationParameters innovationDVL(const Eigen::VectorXd& xnominal, const Eigen::MatrixXd& P, const Eigen::Vector3d& zDVLvel, - const Eigen::Matrix3d& RDVL) const; - InnovationParameters innovationDVLWithLeverArm(const Eigen::VectorXd& xnominal, const Eigen::MatrixXd& P, const Eigen::Vector3d& zDVLvel, - const Eigen::Matrix3d& RDVL) const; + InnovationParameters innovationDVL(const Eigen::VectorXd& xnominal, const Eigen::MatrixXd& P, + const Eigen::Vector3d& zDVLvel, const Eigen::Matrix3d& RDVL) const; + InnovationParameters innovationDVLWithLeverArm(const Eigen::VectorXd& xnominal, const Eigen::MatrixXd& P, + const Eigen::Vector3d& zDVLvel, const Eigen::Matrix3d& RDVL) const; static InnovationParameters innovationPressureZ(const Eigen::VectorXd& xnominal, const Eigen::MatrixXd& P, const double& zPressureZpos, const Eigen::MatrixXd& RpressureZ); @@ -365,15 +374,15 @@ class ESKF Eigen::Matrix3d Rgyro_; // Gyro measurements covariance (3x3) Eigen::Matrix3d RgyroBias_; // Gyro bias driving noise covariance (3x3) - // Eigen::MatrixXd D_ {blk3x3Diag(Racc_, Rgyro_, RaccBias_, RgyroBias_)}; // Diagonal block Eigen::matrix with measurement - // covariances + // Eigen::MatrixXd D_ {blk3x3Diag(Racc_, Rgyro_, RaccBias_, RgyroBias_)}; // Diagonal block Eigen::matrix with + // measurement covariances Eigen::MatrixXd D_; // Correction matricies Eigen::Matrix3d Sa_{ Eigen::Matrix3d::Zero() }; // Accelerometer Eigen::Matrix3d Sg_{ Eigen::Matrix3d::Zero() }; // Gyro Eigen::Matrix3d Sdvl_{ Eigen::Matrix3d::Zero() }; // DVL Eigen::Matrix3d Sinc_{ Eigen::Matrix3d::Zero() }; // Inclinometer - double SpressureZ_{ 0 }; // Pressure + double SpressureZ_{ 0 }; // Pressure StatesAndErrorCovariance optimizationParameters_{}; @@ -389,7 +398,7 @@ class ESKF std::vector dvl_msg_buffer_; std::vector pressureZ_msg_buffer_; std::vector nominal_covariance_buffer_; - //void emptyBuffers(); + // void emptyBuffers(); void emptyIMUBuffer(); void emptyDVLBuffer(); void emptyPressureZBuffer(); @@ -401,8 +410,5 @@ class ESKF // NIS Pressure sensor double NISPressureZ_; double NISDVL_; - - - }; } // namespace eskf diff --git a/navigation/ESKF/src/ESKF.cpp b/navigation/ESKF/src/ESKF.cpp old mode 100755 new mode 100644 index 0a99adfb7..3f8afb46d --- a/navigation/ESKF/src/ESKF.cpp +++ b/navigation/ESKF/src/ESKF.cpp @@ -11,15 +11,15 @@ ESKF::ESKF(Matrix3d Racc, Matrix3d RaccBias, Matrix3d Rgyro, Matrix3d RgyroBias, , Racc_{ std::move(Racc) } , RaccBias_{ std::move(RaccBias) } , Rgyro_{ std::move(Rgyro) } - , updated_{0} + , updated_{ 0 } , RgyroBias_{ std::move(RgyroBias) } , D_{ blk3x3Diag(Racc_, Rgyro_, RaccBias_, RgyroBias_) } , Sa_{ std::move(Sa) } , Sg_{ std::move(Sg) } , Sdvl_{ std::move(Sdvl) } , Sinc_{ std::move(Sinc) } - , NISPressureZ_{0} - , NISDVL_{0} + , NISPressureZ_{ 0 } + , NISDVL_{ 0 } { Vector3d initialPosition{ -1.96, 0.061, 0 }; Vector3d initialVelocity{ 0, 0, 0 }; @@ -46,8 +46,6 @@ ESKF::ESKF(Matrix3d Racc, Matrix3d RaccBias, Matrix3d Rgyro, Matrix3d RgyroBias, optimizationParameters_.P.block<3, 3>(15, 15) = initialPGravity.asDiagonal(); } - - ESKF::ESKF(const parametersInESKF& parameters) : Racc_{ parameters.R_acc } , Rgyro_{ parameters.R_gyro } @@ -55,9 +53,11 @@ ESKF::ESKF(const parametersInESKF& parameters) , RgyroBias_{ parameters.R_gyroBias } , Sdvl_{ eulerToRotationMatrix(parameters.Sr_to_ned_dvl + parameters.Sr_dvl_alignment) } , Sinc_{ parameters.S_inc } - , updated_{0} - , Sa_{ eulerToRotationMatrix(parameters.Sr_to_ned_accelerometer + parameters.Sr_accelerometer_aligment)} //+ eulerToRotationMatrix(parameters.Sr_accelerometer_aligment)} - , Sg_{ eulerToRotationMatrix(parameters.Sr_to_ned_gyro + parameters.Sr_gyro_aligment)} //+ eulerToRotationMatrix(parameters.Sr_gyro_aligment)} + , updated_{ 0 } + , Sa_{ eulerToRotationMatrix(parameters.Sr_to_ned_accelerometer + parameters.Sr_accelerometer_aligment) } + //+ eulerToRotationMatrix(parameters.Sr_accelerometer_aligment)} + , Sg_{ eulerToRotationMatrix(parameters.Sr_to_ned_gyro + parameters.Sr_gyro_aligment) } + //+ eulerToRotationMatrix(parameters.Sr_gyro_aligment)} , pgyroBias_{ parameters.pgyroBias } , paccBias_{ parameters.paccBias } , use_ENU_{ parameters.use_ENU } @@ -71,13 +71,13 @@ ESKF::ESKF(const parametersInESKF& parameters) std::cout << "S_DVL: " << Sdvl_ << std::endl; std::cout << "Sr_to_ned_accelerometer: " << parameters.Sr_to_ned_accelerometer << std::endl; std::cout << "Sr_to_ned_gyro: " << parameters.Sr_to_ned_gyro << std::endl; - std::cout << "Sr_to_ned_DVL: " <(6, 6) = R_wdeltaT.transpose(); @@ -232,8 +231,8 @@ MatrixXd ESKF::Fi() identity_matrix_3x3.setIdentity(); F_i.setZero(); - F_i.block<3, 3>(3, 0) = -1.0*quaternion.toRotationMatrix(); - F_i.block<3, 3>(6, 3) = -1.0*identity_matrix_3x3; + F_i.block<3, 3>(3, 0) = -1.0 * quaternion.toRotationMatrix(); + F_i.block<3, 3>(6, 3) = -1.0 * identity_matrix_3x3; F_i.block<3, 3>(9, 6) = identity_matrix_3x3; F_i.block<3, 3>(12, 9) = identity_matrix_3x3; @@ -262,39 +261,40 @@ AdandGQGD ESKF::discreteErrorMatrix(const VectorXd& xnominal, const Vector3d& ac // Initilize AdandGQGD errorMatrix; - MatrixXd vanLoan(ERROR_STATE_SIZE*2, ERROR_STATE_SIZE*2); - MatrixXd vanLoanExponentional(ERROR_STATE_SIZE*2, ERROR_STATE_SIZE*2); - MatrixXd zeros(ERROR_STATE_SIZE, ERROR_STATE_SIZE); - MatrixXd A(ERROR_STATE_SIZE, ERROR_STATE_SIZE); - MatrixXd G(ERROR_STATE_SIZE, 12); - MatrixXd FDF(ERROR_STATE_SIZE,ERROR_STATE_SIZE); + MatrixXd vanLoan(ERROR_STATE_SIZE * 2, ERROR_STATE_SIZE * 2); + MatrixXd vanLoanExponentional(ERROR_STATE_SIZE * 2, ERROR_STATE_SIZE * 2); + MatrixXd zeros(ERROR_STATE_SIZE, ERROR_STATE_SIZE); + MatrixXd A(ERROR_STATE_SIZE, ERROR_STATE_SIZE); + MatrixXd G(ERROR_STATE_SIZE, 12); + MatrixXd FDF(ERROR_STATE_SIZE, ERROR_STATE_SIZE); - A.setZero(); - G.setZero(); - vanLoanExponentional.setZero(); - zeros.setZero(); - vanLoan.setZero(); - FDF.setZero(); + A.setZero(); + G.setZero(); + vanLoanExponentional.setZero(); + zeros.setZero(); + vanLoan.setZero(); + FDF.setZero(); - //D_ = blk3x3Diag(Racc, Rgyro, RaccBias_, RgyroBias_); + // D_ = blk3x3Diag(Racc, Rgyro, RaccBias_, RgyroBias_); - A = Aerr(xnominal, accRectifiedMeasurements, gyroRectifiedmeasurements); - G = Gerr(xnominal); + A = Aerr(xnominal, accRectifiedMeasurements, gyroRectifiedmeasurements); + G = Gerr(xnominal); - //Calculate Van Loan - vanLoan << -1.0 * A, G* D_* G.transpose(), - zeros, A.transpose(); + // Calculate Van Loan + vanLoan << -1.0 * A, G * D_ * G.transpose(), zeros, A.transpose(); - vanLoan = vanLoan * Ts; + vanLoan = vanLoan * Ts; - //Computation time very slow - vanLoanExponentional= vanLoan.exp(); + // Computation time very slow + vanLoanExponentional = vanLoan.exp(); - errorMatrix.Ad = vanLoanExponentional.block(ERROR_STATE_SIZE, - ERROR_STATE_SIZE).transpose(); - errorMatrix.GQGD = vanLoanExponentional.block(ERROR_STATE_SIZE, ERROR_STATE_SIZE).transpose() * vanLoanExponentional.block(0, ERROR_STATE_SIZE); - //errorMatrix.Ad = AerrDiscretizedThirdOrder(xnominal, accRectifiedMeasurements, gyroRectifiedmeasurements, Ts); - //errorMatrix.GQGD = F_i_ * D_ * F_i_.transpose(); + errorMatrix.Ad = + vanLoanExponentional.block(ERROR_STATE_SIZE, ERROR_STATE_SIZE).transpose(); + errorMatrix.GQGD = + vanLoanExponentional.block(ERROR_STATE_SIZE, ERROR_STATE_SIZE).transpose() * + vanLoanExponentional.block(0, ERROR_STATE_SIZE); + // errorMatrix.Ad = AerrDiscretizedThirdOrder(xnominal, accRectifiedMeasurements, gyroRectifiedmeasurements, Ts); + // errorMatrix.GQGD = F_i_ * D_ * F_i_.transpose(); // Execution time // auto end = std::chrono::steady_clock::now(); @@ -317,23 +317,23 @@ MatrixXd ESKF::predictCovariance(const VectorXd& xnominal, const MatrixXd& P, co return Pprediction; } - - -void ESKF::bufferIMUMessages(const Vector3d& zAccMeasurements, const Vector3d& zGyroMeasurements, const double& timeStamp, const double& deltaIMU,const Matrix3d& Racc, const Matrix3d& Rgyro) +void ESKF::bufferIMUMessages(const Vector3d& zAccMeasurements, const Vector3d& zGyroMeasurements, + const double& timeStamp, const double& deltaIMU, const Matrix3d& Racc, + const Matrix3d& Rgyro) { - IMUmessage imu_msg{timeStamp,deltaIMU,zAccMeasurements,zGyroMeasurements,Racc,Rgyro}; + IMUmessage imu_msg{ timeStamp, deltaIMU, zAccMeasurements, zGyroMeasurements, Racc, Rgyro }; imu_msg_buffer_.push_back(imu_msg); - if(imu_msg_buffer_.size() == 20) + if (imu_msg_buffer_.size() == 20) { emptyIMUBuffer(); } } -void ESKF::bufferDVLMessages(const Vector3d& zDvlMeasurements,const double timeStamp,const Matrix3d& Rdvl) +void ESKF::bufferDVLMessages(const Vector3d& zDvlMeasurements, const double timeStamp, const Matrix3d& Rdvl) { - DVLmessage dvlmsg{timeStamp,zDvlMeasurements,Rdvl}; + DVLmessage dvlmsg{ timeStamp, zDvlMeasurements, Rdvl }; dvl_msg_buffer_.push_back(dvlmsg); @@ -345,9 +345,9 @@ void ESKF::bufferDVLMessages(const Vector3d& zDvlMeasurements,const double timeS */ } -void ESKF::bufferPressureZMessages(const double& pressureZ,const double& timeStamp, Matrix R_pressureZ) +void ESKF::bufferPressureZMessages(const double& pressureZ, const double& timeStamp, Matrix R_pressureZ) { - PressureZmessage prsmsg{timeStamp,pressureZ,R_pressureZ}; + PressureZmessage prsmsg{ timeStamp, pressureZ, R_pressureZ }; pressureZ_msg_buffer_.push_back(prsmsg); @@ -361,7 +361,7 @@ void ESKF::bufferPressureZMessages(const double& pressureZ,const double& timeSta void ESKF::UpdateOnlyWithPrediction() { - if(imu_msg_buffer_.size() != 0) + if (imu_msg_buffer_.size() != 0) { predictWithBuffer(); } @@ -371,42 +371,43 @@ void ESKF::UpdateOnlyWithPrediction() } } - void ESKF::update() { - if(imu_msg_buffer_.size() != 0) + if (imu_msg_buffer_.size() != 0) { - if(dvl_msg_buffer_.size() != 0 && pressureZ_msg_buffer_.size() != 0) + if (dvl_msg_buffer_.size() != 0 && pressureZ_msg_buffer_.size() != 0) { - if(dvl_msg_buffer_.size() == 1 && dvl_msg_buffer_.back().timeStamp_ < pressureZ_msg_buffer_.back().timeStamp_) + if (dvl_msg_buffer_.size() == 1 && dvl_msg_buffer_.back().timeStamp_ < pressureZ_msg_buffer_.back().timeStamp_) { - while(imu_msg_buffer_.front().timeStamp_< dvl_msg_buffer_.back().timeStamp_ && imu_msg_buffer_.size() != 0) + while (imu_msg_buffer_.front().timeStamp_ < dvl_msg_buffer_.back().timeStamp_ && imu_msg_buffer_.size() != 0) { - if(imu_msg_buffer_.front().predicted_msg_ == false) + if (imu_msg_buffer_.front().predicted_msg_ == false) { predictWithBuffer(); } } updateDVLWithBuffer(); emptyDVLBuffer(); - if(pressureZ_msg_buffer_.size() == 1) + if (pressureZ_msg_buffer_.size() == 1) { updatePressureZWithBuffer(); emptyPressureZBuffer(); } } - if(pressureZ_msg_buffer_.size() == 1 && pressureZ_msg_buffer_.back().timeStamp_ < dvl_msg_buffer_.back().timeStamp_) + if (pressureZ_msg_buffer_.size() == 1 && + pressureZ_msg_buffer_.back().timeStamp_ < dvl_msg_buffer_.back().timeStamp_) { - while(imu_msg_buffer_.front().timeStamp_<= pressureZ_msg_buffer_.back().timeStamp_ && imu_msg_buffer_.size() != 0) + while (imu_msg_buffer_.front().timeStamp_ <= pressureZ_msg_buffer_.back().timeStamp_ && + imu_msg_buffer_.size() != 0) { - if(imu_msg_buffer_.front().predicted_msg_ == false) + if (imu_msg_buffer_.front().predicted_msg_ == false) { predictWithBuffer(); } } updatePressureZWithBuffer(); emptyPressureZBuffer(); - if(dvl_msg_buffer_.size() == 1) + if (dvl_msg_buffer_.size() == 1) { updateDVLWithBuffer(); emptyDVLBuffer(); @@ -421,8 +422,6 @@ void ESKF::update() return; } - - /* void ESKF::emptyBuffers() { @@ -434,40 +433,37 @@ void ESKF::emptyBuffers() void ESKF::emptyIMUBuffer() { - imu_msg_buffer_ = std::vector{imu_msg_buffer_.back()}; + imu_msg_buffer_ = std::vector{ imu_msg_buffer_.back() }; - //imu_msg_buffer_.back() + // imu_msg_buffer_.back() } void ESKF::emptyDVLBuffer() { dvl_msg_buffer_ = std::vector{}; - //dvl_msg_buffer_.back() + // dvl_msg_buffer_.back() } void ESKF::emptyPressureZBuffer() { pressureZ_msg_buffer_ = std::vector{}; - //pressureZ_msg_buffer_.back() + // pressureZ_msg_buffer_.back() } - - void ESKF::predictWithBuffer() { - Vector3d gyroMessage{imu_msg_buffer_.front().zGyroMeasurement_}; - Vector3d accMessage{imu_msg_buffer_.front().zAccMeasurement_}; - Matrix Racc{imu_msg_buffer_.front().R_acc_}; - Matrix Rgyro{imu_msg_buffer_.front().R_gyro_}; - - const double Ts{imu_msg_buffer_.front().deltaIMU_}; - + Vector3d gyroMessage{ imu_msg_buffer_.front().zGyroMeasurement_ }; + Vector3d accMessage{ imu_msg_buffer_.front().zAccMeasurement_ }; + Matrix Racc{ imu_msg_buffer_.front().R_acc_ }; + Matrix Rgyro{ imu_msg_buffer_.front().R_gyro_ }; - Vector3d accBias = Sa_*optimizationParameters_.X.block(NOMINAL_ACC_BIAS_STATE_OFFSET, 0); - Vector3d gyroBias = Sg_*optimizationParameters_.X.block(NOMINAL_GYRO_BIAS_STATE_OFFSET, 0); + const double Ts{ imu_msg_buffer_.front().deltaIMU_ }; + Vector3d accBias = Sa_ * optimizationParameters_.X.block(NOMINAL_ACC_BIAS_STATE_OFFSET, 0); + Vector3d gyroBias = + Sg_ * optimizationParameters_.X.block(NOMINAL_GYRO_BIAS_STATE_OFFSET, 0); Vector3d accelerationRectified = (Sa_ * accMessage) - accBias; Vector3d gyroRectified = (Sg_ * gyroMessage) - gyroBias; @@ -481,34 +477,28 @@ void ESKF::predictWithBuffer() accelerationRectified, gyroRectified, Ts, Racc, Rgyro); // Add state and covariance to buffer - //const double time_Stamp{imu_msg_buffer_.back().timeStamp_}; - //StateAndCovariance_msg state_and_covariance_msg{optimizationParameters_.X,optimizationParameters_.P,time_Stamp}; - //nominal_covariance_buffer_.push_back(state_and_covariance_msg); + // const double time_Stamp{imu_msg_buffer_.back().timeStamp_}; + // StateAndCovariance_msg state_and_covariance_msg{optimizationParameters_.X,optimizationParameters_.P,time_Stamp}; + // nominal_covariance_buffer_.push_back(state_and_covariance_msg); imu_msg_buffer_.front().predicted_msg_ = true; imu_msg_buffer_.erase(imu_msg_buffer_.begin()); - - } - void ESKF::predict(const Vector3d& zAccMeasurements, const Vector3d& zGyroMeasurements, const double& Ts, const Matrix3d& Racc, const Matrix3d& Rgyro) { - - // NB: Changed Sa_ - Vector3d accBias = Sa_*optimizationParameters_.X.block(NOMINAL_ACC_BIAS_STATE_OFFSET, 0); - Vector3d gyroBias = Sg_*optimizationParameters_.X.block(NOMINAL_GYRO_BIAS_STATE_OFFSET, 0); + // NB: Changed Sa_ + Vector3d accBias = Sa_ * optimizationParameters_.X.block(NOMINAL_ACC_BIAS_STATE_OFFSET, 0); + Vector3d gyroBias = + Sg_ * optimizationParameters_.X.block(NOMINAL_GYRO_BIAS_STATE_OFFSET, 0); - Vector3d accelerationRectified = (Sa_* zAccMeasurements) - accBias; + Vector3d accelerationRectified = (Sa_ * zAccMeasurements) - accBias; Vector3d gyroRectified = (Sg_ * zGyroMeasurements) - gyroBias; optimizationParameters_.X = predictNominal(optimizationParameters_.X, accelerationRectified, gyroRectified, Ts); optimizationParameters_.P = predictCovariance(optimizationParameters_.X, optimizationParameters_.P, accelerationRectified, gyroRectified, Ts, Racc, Rgyro); - - - } StatesAndErrorCovariance ESKF::inject(const VectorXd& xnominal, const VectorXd& deltaX, const MatrixXd& P) const @@ -574,11 +564,12 @@ void ESKF::updatePressureZ(const double& zPressureZpos, const MatrixXd& Rpressur InnovationParameters pressureStates = innovationPressureZ(optimizationParameters_.X, optimizationParameters_.P, zPressureZpos, RpressureZ); - Matrix NIS_pressureZ; + Matrix NIS_pressureZ; NIS_pressureZ.setZero(); // Calculate NISPressureZ - NIS_pressureZ = pressureStates.measurementStates.transpose()*pressureStates.measurementCovariance.inverse()*pressureStates.measurementStates; + NIS_pressureZ = pressureStates.measurementStates.transpose() * pressureStates.measurementCovariance.inverse() * + pressureStates.measurementStates; NISPressureZ_ = NIS_pressureZ(0); @@ -593,16 +584,17 @@ void ESKF::updatePressureZ(const double& zPressureZpos, const MatrixXd& Rpressur void ESKF::updatePressureZWithBuffer() { - const double prs_msg{pressureZ_msg_buffer_.back().pressureZ_msg_}; - const Matrix R_pressureZ{pressureZ_msg_buffer_.back().R_pressureZ_}; - Matrix NIS_pressureZ; + const double prs_msg{ pressureZ_msg_buffer_.back().pressureZ_msg_ }; + const Matrix R_pressureZ{ pressureZ_msg_buffer_.back().R_pressureZ_ }; + Matrix NIS_pressureZ; NIS_pressureZ.setZero(); InnovationParameters pressureStates = - innovationPressureZ(optimizationParameters_.X, optimizationParameters_.P,prs_msg,R_pressureZ); + innovationPressureZ(optimizationParameters_.X, optimizationParameters_.P, prs_msg, R_pressureZ); // Calculate NISPressureZ - NIS_pressureZ = pressureStates.measurementStates.transpose()*pressureStates.measurementCovariance.inverse()*pressureStates.measurementStates; + NIS_pressureZ = pressureStates.measurementStates.transpose() * pressureStates.measurementCovariance.inverse() * + pressureStates.measurementStates; NISPressureZ_ = NIS_pressureZ(0); @@ -650,8 +642,8 @@ InnovationParameters ESKF::innovationDVL(const VectorXd& xnominal, const MatrixX return dvlStates; } -InnovationParameters ESKF::innovationDVLWithLeverArm(const VectorXd& xnominal, const MatrixXd& P, const Vector3d& zDVLvel, - const Matrix3d& RDVL) const +InnovationParameters ESKF::innovationDVLWithLeverArm(const VectorXd& xnominal, const MatrixXd& P, + const Vector3d& zDVLvel, const Matrix3d& RDVL) const { Vector3d vel_world = xnominal.block(NOMINAL_VELOCITY_STATE_OFFSET, 0); Quaterniond nominalQuaternion{ optimizationParameters_.X(StateMemberQw), optimizationParameters_.X(StateMemberQx), @@ -676,30 +668,31 @@ InnovationParameters ESKF::innovationDVLWithLeverArm(const VectorXd& xnominal, c MatrixXd X_deltaX{ MatrixXd::Identity(NOMINAL_STATE_SIZE, ERROR_STATE_SIZE) }; X_deltaX.block<4, 3>(6, 6) = Q_deltaT; - Vector3d leverarm {0.014, 0.0, 0.166}; //Lever arm for beluga //-0.035,-0.017,-0.211 -0.017,-0.035,0.211 + Vector3d leverarm{ 0.014, 0.0, 0.166 }; // Lever arm for beluga //-0.035,-0.017,-0.211 -0.017,-0.035,0.211 InnovationParameters dvlStates(3); dvlStates.jacobianOfErrorStates = Hx * X_deltaX; - dvlStates.measurementStates = (zDVLvel + crossProductMatrix(leverarm)*gyro_msg_in_dvl_compensation_) - Hv * vel_world; + dvlStates.measurementStates = + (zDVLvel + crossProductMatrix(leverarm) * gyro_msg_in_dvl_compensation_) - Hv * vel_world; dvlStates.measurementCovariance = (dvlStates.jacobianOfErrorStates * P * dvlStates.jacobianOfErrorStates.transpose()) + RDVL; return dvlStates; } - void ESKF::updateDVLWithBuffer() { - const Vector3d dvl_msg{dvl_msg_buffer_.back().zDVl_}; - const Matrix3d R_dvl{dvl_msg_buffer_.back().R_dvl_}; - Matrix NIS_DVL; + const Vector3d dvl_msg{ dvl_msg_buffer_.back().zDVl_ }; + const Matrix3d R_dvl{ dvl_msg_buffer_.back().R_dvl_ }; + Matrix NIS_DVL; NIS_DVL.setZero(); InnovationParameters DVLstates{ 3 }; DVLstates = innovationDVLWithLeverArm(optimizationParameters_.X, optimizationParameters_.P, dvl_msg, R_dvl); // Calculate NISPressureZ - NIS_DVL = DVLstates.measurementStates.transpose()*DVLstates.measurementCovariance.inverse()*DVLstates.measurementStates; + NIS_DVL = + DVLstates.measurementStates.transpose() * DVLstates.measurementCovariance.inverse() * DVLstates.measurementStates; NISDVL_ = NIS_DVL(0); @@ -711,15 +704,15 @@ void ESKF::updateDVLWithBuffer() optimizationParameters_ = inject(optimizationParameters_.X, deltaX, pUpdate); } - void ESKF::updateDVL(const Vector3d& zDVLvel, const Matrix3d& RDVL) { - Matrix NIS_DVL; + Matrix NIS_DVL; NIS_DVL.setZero(); InnovationParameters DVLstates{ 3 }; DVLstates = innovationDVL(optimizationParameters_.X, optimizationParameters_.P, zDVLvel, RDVL); - NIS_DVL = DVLstates.measurementStates.transpose()*DVLstates.measurementCovariance.inverse()*DVLstates.measurementStates; + NIS_DVL = + DVLstates.measurementStates.transpose() * DVLstates.measurementCovariance.inverse() * DVLstates.measurementStates; NISDVL_ = NIS_DVL(0); diff --git a/navigation/compass/src/compass.py b/navigation/compass/src/compass.py index 2f8cad9a0..91eb4a605 100755 --- a/navigation/compass/src/compass.py +++ b/navigation/compass/src/compass.py @@ -6,26 +6,32 @@ from sensor_msgs.msg import MagneticField from std_msgs.msg import Float32 + class Compass: def __init__(self): - """simple compass that subscribes to a magnetic field and publishes heading. Assumes no tilt. - """ - + """simple compass that subscribes to a magnetic field and publishes heading. Assumes no tilt.""" + # parameters - magnetometer_topic = rospy.get_param("magnetometer_topic", default='/zed2/zed_node/imu/mag') + magnetometer_topic = rospy.get_param( + "magnetometer_topic", default="/zed2/zed_node/imu/mag" + ) heading_topic = rospy.get_param("heading_topic", default="heading_magnetic") - + # set up sub and pub self.compass_pub = rospy.Publisher(heading_topic, Float32, queue_size=1) - self.compass_pub_deg = rospy.Publisher(heading_topic + "_deg", Float32, queue_size=1) - self.magnetometer_sub = rospy.Subscriber(magnetometer_topic, MagneticField, self.magnetometer_cb) - + self.compass_pub_deg = rospy.Publisher( + heading_topic + "_deg", Float32, queue_size=1 + ) + self.magnetometer_sub = rospy.Subscriber( + magnetometer_topic, MagneticField, self.magnetometer_cb + ) def magnetometer_cb(self, mag_msg): heading = atan2(mag_msg.magnetic_field.y, mag_msg.magnetic_field.x) self.compass_pub.publish(heading) self.compass_pub_deg.publish(degrees(heading)) + if __name__ == "__main__": rospy.init_node("compass") compass = Compass() diff --git a/navigation/depth_estimator/include/depth_estimator/depth_estimator.h b/navigation/depth_estimator/include/depth_estimator/depth_estimator.h index 373a730f1..e4b354ae4 100644 --- a/navigation/depth_estimator/include/depth_estimator/depth_estimator.h +++ b/navigation/depth_estimator/include/depth_estimator/depth_estimator.h @@ -3,13 +3,12 @@ #include -#include #include -#include +#include #include +#include -class DepthEstimator -{ +class DepthEstimator { public: DepthEstimator(ros::NodeHandle nh); @@ -21,8 +20,8 @@ class DepthEstimator double atmospheric_pressure; double water_density; double earth_gravitation; - - void pressureCallback(const sensor_msgs::FluidPressure& msg); + + void pressureCallback(const sensor_msgs::FluidPressure &msg); }; #endif diff --git a/navigation/depth_estimator/src/depth_estimator.cpp b/navigation/depth_estimator/src/depth_estimator.cpp index 902d800a8..a6c53c4bc 100644 --- a/navigation/depth_estimator/src/depth_estimator.cpp +++ b/navigation/depth_estimator/src/depth_estimator.cpp @@ -1,25 +1,21 @@ #include "depth_estimator/depth_estimator.h" -DepthEstimator::DepthEstimator(ros::NodeHandle nh) : nh(nh) -{ +DepthEstimator::DepthEstimator(ros::NodeHandle nh) : nh(nh) { // params std::string pressure_topic; std::string depth_topic; - if (!nh.getParam("/atmosphere/pressure", atmospheric_pressure)) - { + if (!nh.getParam("/atmosphere/pressure", atmospheric_pressure)) { ROS_ERROR("Could not read parameter atmospheric pressure."); ros::shutdown(); } - if (!nh.getParam("/water/density", water_density)) - { + if (!nh.getParam("/water/density", water_density)) { ROS_ERROR("Could not read parameter water density."); ros::shutdown(); } - - if (!nh.getParam("/gravity/acceleration", earth_gravitation)) - { + + if (!nh.getParam("/gravity/acceleration", earth_gravitation)) { ROS_ERROR("Could not read parameter gravititional acceleration."); ros::shutdown(); } @@ -32,13 +28,14 @@ DepthEstimator::DepthEstimator(ros::NodeHandle nh) : nh(nh) // subscriber and publsiher depth_pub = nh.advertise("/depth/estimator", 1); - pressure_sub = nh.subscribe(pressure_topic, 1, &DepthEstimator::pressureCallback, this); + pressure_sub = + nh.subscribe(pressure_topic, 1, &DepthEstimator::pressureCallback, this); } -void DepthEstimator::pressureCallback(const sensor_msgs::FluidPressure& msg) -{ +void DepthEstimator::pressureCallback(const sensor_msgs::FluidPressure &msg) { const double mbar_pressure = msg.fluid_pressure / 10.0; - const double depth_mm = mbar_pressure * 10.197; // 10.197 is the constant to convert mbar to mmH2O + const double depth_mm = + mbar_pressure * 10.197; // 10.197 is the constant to convert mbar to mmH2O const double depth_meters = depth_mm / 1000.0; std_msgs::Float32 depth_msg; diff --git a/navigation/depth_estimator/src/depth_estimator_node.cpp b/navigation/depth_estimator/src/depth_estimator_node.cpp index 4d0c50ed1..b389629c5 100644 --- a/navigation/depth_estimator/src/depth_estimator_node.cpp +++ b/navigation/depth_estimator/src/depth_estimator_node.cpp @@ -1,7 +1,6 @@ #include "depth_estimator/depth_estimator.h" -int main(int argc, char** argv) -{ +int main(int argc, char **argv) { ros::init(argc, argv, "depth_estimator"); ros::NodeHandle nh; DepthEstimator depth_estimator(nh); diff --git a/navigation/inspect_point/src/inspect_point.py b/navigation/inspect_point/src/inspect_point.py index 039cbe8ee..21f40b3f9 100755 --- a/navigation/inspect_point/src/inspect_point.py +++ b/navigation/inspect_point/src/inspect_point.py @@ -1,11 +1,11 @@ #!/usr/bin/python3 -#python imports +# python imports import rospy import numpy as np import math -#action lib +# action lib import actionlib from actionlib_msgs.msg import GoalStatus @@ -22,12 +22,11 @@ from vortex_msgs.msg import MoveGoal, MoveAction - class InspectPoint: def __init__(self): # init node - rospy.init_node('inspect_unknown_point') + rospy.init_node("inspect_unknown_point") # init variables self.run_controller = False @@ -61,18 +60,22 @@ def __init__(self): self.PID = LOSControllerPID() # subscribers - self.sub_pose = rospy.Subscriber('/odometry/filtered', Odometry, self.positionCallback, queue_size=1) + self.sub_pose = rospy.Subscriber( + "/odometry/filtered", Odometry, self.positionCallback, queue_size=1 + ) # publishers - self.pub_thrust = rospy.Publisher('/auv/thruster_manager/input', Wrench, queue_size=1) + self.pub_thrust = rospy.Publisher( + "/auv/thruster_manager/input", Wrench, queue_size=1 + ) # action server setup - self.action_server = actionlib.SimpleActionServer(name='inspect_point', ActionSpec=MoveAction, auto_start=False) + self.action_server = actionlib.SimpleActionServer( + name="inspect_point", ActionSpec=MoveAction, auto_start=False + ) self.action_server.register_goal_callback(self.goalCB) self.action_server.start() - - def fixHeadingWrapping(self, err_yaw): """ Fix error in yaw such that the auv rotates in the closest direction @@ -80,19 +83,18 @@ def fixHeadingWrapping(self, err_yaw): """ if err_yaw > math.pi: - err_yaw = err_yaw - 2*math.pi + err_yaw = err_yaw - 2 * math.pi if err_yaw < -math.pi: - err_yaw = err_yaw + 2*math.pi + err_yaw = err_yaw + 2 * math.pi return err_yaw - def getVectorFromAUVToCentre(self): """ Get vector from auv to the centre point of the object it is inspecting """ - vec_to_mid = np.array([0,0]) + vec_to_mid = np.array([0, 0]) vec_to_mid[0] = self.centre_of_rot.x - self.x vec_to_mid[1] = self.centre_of_rot.y - self.y @@ -103,17 +105,19 @@ def distanceToMid(self): """ Get distance to the centre point of the object it is inspecting """ - return np.sqrt(abs(self.x-self.centre_of_rot.x)**2 + abs(self.y - self.centre_of_rot.y)**2) + return np.sqrt( + abs(self.x - self.centre_of_rot.x) ** 2 + + abs(self.y - self.centre_of_rot.y) ** 2 + ) def getYawFrom2Pos(self, pos1, pos2): """ Get the angle between two points expressed in the global frame """ - return np.arctan2((pos2.y - pos1.y),(pos2.x - pos1.x)) - + return np.arctan2((pos2.y - pos1.y), (pos2.x - pos1.x)) def controller(self): - + # distance control dist = self.distanceToMid() @@ -123,20 +127,21 @@ def controller(self): vec_to_mid_glob_frame = self.getVectorFromAUVToCentre() - rot_mat = np.array([[np.cos(self.yaw), -np.sin(self.yaw)],[np.sin(self.yaw), np.cos(self.yaw)]]) + rot_mat = np.array( + [ + [np.cos(self.yaw), -np.sin(self.yaw)], + [np.sin(self.yaw), np.cos(self.yaw)], + ] + ) rot_mat = rot_mat.transpose() - vec_to_mid_auv_frame = np.dot(rot_mat,vec_to_mid_glob_frame) - + vec_to_mid_auv_frame = np.dot(rot_mat, vec_to_mid_glob_frame) force_vec = vec_to_mid_auv_frame * pid_output force_x = force_vec[0] force_y = force_vec[1] - - - # angle control auv_pos = Point() auv_pos.x = self.x @@ -154,12 +159,14 @@ def controller(self): max_side_force = 5 - sideway_force = max(0, max_side_force - 5*err_yaw) + sideway_force = max(0, max_side_force - 5 * err_yaw) force_y = force_y + sideway_force - + # depth control - tau_depth_hold = self.PID.depthController(self.desired_depth, self.z, rospy.get_time()) + tau_depth_hold = self.PID.depthController( + self.desired_depth, self.z, rospy.get_time() + ) # make wrench and publish wrench = Wrench() @@ -171,35 +178,35 @@ def controller(self): self.pub_thrust.publish(wrench) PRINT_INFO = False - if PRINT_INFO: - print("Force_x: ", force_x, " Force_y: ", force_y, " torque_z: ", torque_z) + if PRINT_INFO: + print( + "Force_x: ", force_x, " Force_y: ", force_y, " torque_z: ", torque_z + ) print("Error distance: ", err_dist, " Error yaw: ", err_yaw) - def goalCB(self): """ - Callback that gets called when the server + Callback that gets called when the server """ print("got goal") self.run_controller = True goal = self.action_server.accept_new_goal() self.centre_of_rot.x = goal.target_pose.position.x - self.centre_of_rot.y = goal.target_pose.position.y + self.centre_of_rot.y = goal.target_pose.position.y self.desired_radius = goal.radius_of_acceptance print("cor x: ", self.centre_of_rot.x) print("cor y: ", self.centre_of_rot.y) print("des rad:", self.desired_radius) - - def distanceBetweenPoseAndSelf(self, pose): """ Distance from a pose to the auv """ - return np.sqrt(self.x-pose.position.x)**2 + abs(self.y - pose.position.y)**2 - + return ( + np.sqrt(self.x - pose.position.x) ** 2 + abs(self.y - pose.position.y) ** 2 + ) def positionCallback(self, msg): """ @@ -210,8 +217,13 @@ def positionCallback(self, msg): self.time = msg.header.stamp.to_sec() global roll, pitch, yaw orientation_q = msg.pose.pose.orientation - orientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w] - (roll,pitch,yaw) = euler_from_quaternion(orientation_list) + orientation_list = [ + orientation_q.x, + orientation_q.y, + orientation_q.z, + orientation_q.w, + ] + (roll, pitch, yaw) = euler_from_quaternion(orientation_list) self.x = msg.pose.pose.position.x self.y = msg.pose.pose.position.y @@ -221,17 +233,14 @@ def positionCallback(self, msg): self.pitch = pitch self.yaw = yaw - # run controller if self.run_controller: self.controller() - - -if __name__ == '__main__': - try: - insepctObj = InspectPoint() - rospy.spin() - except rospy.ROSInterruptException: - pass +if __name__ == "__main__": + try: + insepctObj = InspectPoint() + rospy.spin() + except rospy.ROSInterruptException: + pass diff --git a/navigation/qualisys_motion_capture/LICENSE b/navigation/qualisys_motion_capture/LICENSE deleted file mode 100644 index d64569567..000000000 --- a/navigation/qualisys_motion_capture/LICENSE +++ /dev/null @@ -1,202 +0,0 @@ - - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright [yyyy] [name of copyright owner] - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. diff --git a/navigation/qualisys_motion_capture/README.md b/navigation/qualisys_motion_capture/README.md deleted file mode 100644 index b0077b463..000000000 --- a/navigation/qualisys_motion_capture/README.md +++ /dev/null @@ -1,106 +0,0 @@ -# ROS Driver for Motion Capture Systems -![VICON Logo](http://www.awn.com/sites/default/files/styles/inline_medium/public/image/featured/1025139-vicon-delivers-motion-capture-innovations-siggraph-2015.jpg?itok=vsH7Prwo) - -![QUALISYS Logo](https://cdn-content.qualisys.com/2021/01/qualisys-logo-530x.png) - -This package contains ROS drivers for two different motion capture systems,**VICON** And **QUALISYS**. -**NOTE:** This fork focuses on the Qualisys ROS driver. Some alterations have been made to the Vicon package to ensure compatability with the mutual interface. - -## License -For the VICON driver, we use the [offical SDK](http://www.vicon.com/products/software/datastream-sdk). - -For the QUALISYS driver, we use the [Qualisys CPP SDK](https://github.com/qualisys/qualisys_cpp_sdk). - -For the rest of the software, the license is Apache 2.0 wherever not specified. - -## Compiling -This is a catkin package. Make sure the package is on `ROS_PACKAGE_PATH` after cloning the package to your workspace. Drivers for different motion capture system can be independently compiled. - -``` -cd your_work_space -catkin_make --pkg mocap_{sys} --cmake-args -DCMAKE_BUILD_TYPE=Release -``` - -This will compile the drivers for `{sys}` - -## Example Usage - -**Common Parameters** - -`server_address` (`string`) - -IP address of the server of the motion capture system to be connected. - -`server_base_port` (`string`, `default: 22222`) -The port used for setting up the connection with the motion capture server. -Should be left to the default value. - -`frame_rate` (`int`, `default: 0`) - -The frame rate of the motion capture system. A value of 0 uses whatever framerate -the motion capture server is transmiting at. If lower frame rates are requested the -server will drop frames to get close to the requested rate. For example, if the server -is capturing at 100 FPS and 80 FPS is requested, every other frame will be dropped -resulting in 50 FPS. - -`max_accel` (`double`, `default: 10.0`) - -The max possible acceleration which serves to construct the noise parameters. - -`publish_tf` (`bool`, `default: false`) - -If set to true, tf msgs for the subjects are published. - -`fixed_frame_id` (`string`, `default: mocap`) - -The fixed frame ID of the tf msgs for each subject. Note that the child frame id is automatically set to the name of the subject. - -`udp_port` (`int`, `default: 0`) - -The UDP port that the server will send data to. If set to 0 a random valid port is used. If set to -1 -TCP will be used instead of UDP. - -`qtm_protocol_version` (`int`, `default: 18`) - -The minor version of the QTM protocol that should be used (the only valid major version is 1). -Might have to be lowered for very old versions of QTM. should not be set lower than 8 due to a bug in -the protocol version 1.7 and prior causing the rotations to be incorrectly searialized. - -`model_list` (`vector`, `default: []`) - -A vector of subjects of interest. Leave the vector empty if all subjects are to be tracked. - -**Published Topics** - -`/{mocap_sys}/{subject_name}/odom` (`nav_msgs/Odometry`) - -Odometry message for each specified subject in `model_list`. - -To be compatible with the name of the topics published of `vicon_odom` in [vicon repo of KumarRobotics](https://github.com/KumarRobotics/vicon), you can uncomment the following line in the launch file: -`` - -**Node** - -`roslaunch {mocap_sys} {sys}.launch` - -For example, - -`roslaunch mocap_qualisys qualisys.launch` - -A node for making comparisions between the mocap system and for example the internal odometry of a robot -is also provided -`roslaunch mocap_qualisys comp_qualisys.launch` -The settings will ofcourse have to be tweaked to suit your purpose. - -## FAQ - -1. Will the msgs be delayed if the driver is handling several subjects? - Possibly. It's not recommended to for example, try to run the node for two dozen bodies on a raspberry pie. - The multithreading in earlier versions were removed since it often caused more overhhead than actual gain. - -2. How to calibrate the transformation between the subject frame (centered at the centroid of the markers) and the body frame of a robot? - - -## Bug Report - -Open an issue. diff --git a/navigation/qualisys_motion_capture/mocap_base/CMakeLists.txt b/navigation/qualisys_motion_capture/mocap_base/CMakeLists.txt deleted file mode 100644 index 98aac5d01..000000000 --- a/navigation/qualisys_motion_capture/mocap_base/CMakeLists.txt +++ /dev/null @@ -1,61 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(mocap_base) - -set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/cmake") - -find_package(Boost REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(catkin REQUIRED COMPONENTS - roscpp - geometry_msgs - eigen_conversions -) - -catkin_package( - INCLUDE_DIRS include - LIBRARIES mocap_base_driver mocap_kalman_filter - CATKIN_DEPENDS - geometry_msgs - DEPENDS - Boost EIGEN3 -) - -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} -) - -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall") - -add_library(mocap_kalman_filter - src/KalmanFilter.cpp -) -target_link_libraries(mocap_kalman_filter - ${catkin_LIBRARIES} -) -add_dependencies(mocap_kalman_filter - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) - -add_library(mocap_base_driver - src/MoCapDriverBase.cpp -) -target_link_libraries(mocap_base_driver - ${catkin_LIBRARIES} - mocap_kalman_filter -) -add_dependencies(mocap_base_driver - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) - -install(TARGETS mocap_base_driver mocap_kalman_filter - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) diff --git a/navigation/qualisys_motion_capture/mocap_base/include/mocap_base/KalmanFilter.h b/navigation/qualisys_motion_capture/mocap_base/include/mocap_base/KalmanFilter.h deleted file mode 100644 index 5f10b8454..000000000 --- a/navigation/qualisys_motion_capture/mocap_base/include/mocap_base/KalmanFilter.h +++ /dev/null @@ -1,153 +0,0 @@ -/* - * Copyright [2015] [Ke Sun ] - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef MOCAP_KALMAN_FILTER_H -#define MOCAP_KALMAN_FILTER_H - -#include -#include - -namespace mocap{ - -/* - * @brief Error state Kalman filter to estimate the pose - * of the poses within the arena of the motion capture system - * - * For more details, please refer to - * Nikolas Trawny and Stregios I. Roumeliotis, "Indirect Kalman - * Filter for 3D Attitude Estimation: A Tutorial for Quaternion - * Algebra", Technical Report Number 2005-002, Rev. 57, Mar. 2005 - */ -class KalmanFilter { - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - typedef Eigen::Matrix Matrix6d; - typedef Eigen::Matrix Matrix12d; - typedef Eigen::Matrix Matrix6_12d; - typedef Eigen::Matrix Matrix12_6d; - typedef Eigen::Matrix Vector6d; - typedef Eigen::Matrix Vector12d; - - // NOTE: These variables are left public for quick access - // Do NOT modify them outsize the class - - // State of a rigid body - Eigen::Quaterniond attitude; // Takes a vector from inertia frame to body frame - Eigen::Vector3d position; // In inertia frame - Eigen::Vector3d angular_vel; // In body frame - Eigen::Vector3d linear_vel; // In inertia frame - - Matrix12d state_cov; // State uncertainty - Matrix12d input_cov; // Input uncertainty - Matrix6d measurement_cov; // Measurement uncertainty - - /* - * @brief Constructor and Destructor - */ - KalmanFilter(); - ~KalmanFilter() { - return; - } - - /* - * @brief init Initialize the kalman filter - * @param u_cov Input noise - * @param m_cov Measurement noise - * @return True if success - */ - bool init(const Matrix12d& u_cov, - const Matrix6d& m_cov, const int& freq); - - /* - * @brief prepareInitialCondition This functions is dedicated - * to create the intial condition for the kalman filter. The - * function needs to be called twice, once for initializaing - * the pose and once for the angular and linear velocities - * @param m_attitude Measured atttiude from the motion - * capture system - * @param m_position Measured posiiton from the motion - * capture system - * @return True if the input parameters are accepted; False if - * the filter has already been intialized - */ - bool prepareInitialCondition(const double& curr_time_stamp, - const Eigen::Quaterniond& m_attitude, - const Eigen::Vector3d& m_position); - - /* - * @brief isReady Tells if the filter is ready - * @return True if ready - */ - bool isReady(); - - /* - * @brief reset Resets the filter. After reset(), the - * filter needs to be re-initialized. - */ - void reset(); - - /* - * @brief prediction Performs the predictions step - * of the Kalman filter - * @param curr_time_stamp Current time - */ - void prediction(const double& curr_time_stamp); - - /* - * @brief update Performs the measurement update step - * of the Kalman filter - * @param m_attitude Measured atttiude from the motion - * capture system - * @param m_position Measured posiiton from the motion - * capture system - */ - void update(const Eigen::Quaterniond& m_attitude, - const Eigen::Vector3d& m_position); - - private: - // Disable copy constructor and assign operator for the class - KalmanFilter(const KalmanFilter&); - KalmanFilter& operator=(const KalmanFilter&); - - // Status of the filter during initialization - // The filter requires two sets of measurements in order - // to compute a good initial pose for the rigid body - enum Status{ - INIT_POSE, - INIT_TWIST, - READY - }; - - // Status of the filter - Status filter_status; - - // Time that the state correspond to - double last_time_stamp; - // Expected time interval between updates - double msg_interval; - - // Jacobians - Matrix12d proc_jacob; // Process jacobian - Matrix6_12d meas_jacob; // Measurement jacobian - Matrix12d proc_noise_jacob; // Process noise jacobian - Matrix6d meas_noise_jacob; // Measurement noise jacobian -}; - -} - -#endif diff --git a/navigation/qualisys_motion_capture/mocap_base/include/mocap_base/MoCapDriverBase.h b/navigation/qualisys_motion_capture/mocap_base/include/mocap_base/MoCapDriverBase.h deleted file mode 100644 index 8d29bffc7..000000000 --- a/navigation/qualisys_motion_capture/mocap_base/include/mocap_base/MoCapDriverBase.h +++ /dev/null @@ -1,217 +0,0 @@ -/* - * Copyright [2015] [Ke Sun ] - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef MOCAP_DRIVER_BASE_H -#define MOCAP_DRIVER_BASE_H - - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace mocap{ - -/* - * @brief Subject Defines attributes of a rigid body - */ -class Subject { - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - enum Status { - LOST, - INITIALIZING, - TRACKED - }; - - /* - * @brief Constructor and Destructor - */ - Subject(ros::NodeHandle* nptr, const std::string& sub_name, - const std::string& p_frame); - ~Subject() {} - - /* - * @brief getName setName - * Get and set the name of the object - */ - const std::string& getName(); - void setName(const std::string& sub_name); - /* - * @brief isActive Tells if the object is still active or not - */ - const Status& getStatus(); - void enable(); - void disable(); - /* - * @brief getAttitude getPosition getAngularVel getLinearVel - * Returns the state of the object - */ - const Eigen::Quaterniond& getAttitude(); - const Eigen::Vector3d& getPosition(); - const Eigen::Vector3d& getAngularVel(); - const Eigen::Vector3d& getLinearVel(); - - /* - * @brief setNoiseParameter Set noise parameters for - * the kalman filter - * @param u_cov Input noise - * @param m_cov Measurement noise - * @return True if success - */ - bool setParameters( - const Eigen::Matrix& u_cov, - const Eigen::Matrix& m_cov, - const int& freq); - - /* - * @brief processNewMeasurement Process new measurements - * from the mocap system - * @param m_attitude Measured attitude - * @param m_position Measured position - */ - void processNewMeasurement( - const double& time, - const Eigen::Quaterniond& m_attitude, - const Eigen::Vector3d& m_position); - - - typedef boost::shared_ptr SubjectPtr; - typedef const boost::shared_ptr SubjectConstPtr; - - private: - // Disable copy constructor and assign operator - Subject(const Subject&); - Subject& operator=(const Subject&); - - // Name of the subject - std::string name; - - // Error state Kalman filter - KalmanFilter kFilter; - - // Tells the status of the object - Status status; - - // Prevent cocurrent reading and writing of the class - boost::shared_mutex mtx; - - // Publisher for the subject - ros::NodeHandle* nh_ptr; - std::string parent_frame; - ros::Publisher pub_filter; - ros::Publisher pub_raw; - ros::Publisher pub_vel; -}; - -/* - * @brief: MoCapDriverBase Base class for the drivers - * of different motion capture system (e.g. vicon - * and qualisys) - */ -class MoCapDriverBase{ - - public: - /* - * @brief Constructor - * @param nh Ros node - */ - MoCapDriverBase(const ros::NodeHandle& n): - nh (n), - frame_rate (100), - model_list (std::vector(0)), - publish_tf (false), - fixed_frame_id ("mocap"){ - return; - } - - /* - * @brief Destructor - */ - ~MoCapDriverBase() { - return; - } - - /* - * @brief init Initialize the object - * @return True if successfully initialized - */ - virtual bool init() = 0; - - /* - * @brief run Start acquiring data from the server once - */ - virtual bool run() = 0; - - /* - * @brief disconnect Disconnect to the server - * @Note The function is called automatically when the - * destructor is called. - */ - virtual void disconnect() = 0; - - private: - // Disable the copy constructor and assign operator - MoCapDriverBase(const MoCapDriverBase& ); - MoCapDriverBase& operator=(const MoCapDriverBase& ); - - protected: - /* - * @brief handleFrame handles a whole data package from - * the motion capture system - */ - virtual void handleFrame() = 0; - - /* - * @brief handSubject Handles a single subject (rigid body) - * from the motion capture system - * @param sub_idx Index of the subject to be handled - */ - virtual void handleSubject(int sub_idx) = 0; - - // Address of the server - std::string server_address; - - // Ros node - ros::NodeHandle nh; - - // Frame rate of the mocap system - unsigned int frame_rate; - - // Rigid body to be tracked - // Empty string if all the objects in the arena are to be tracked - std::vector model_list; - - // Observed rigid bodies (contains those lose tracking) - std::map subjects; - - // Publish tf - bool publish_tf; - std::string fixed_frame_id; - tf::TransformBroadcaster tf_publisher; - -}; -} - - -#endif diff --git a/navigation/qualisys_motion_capture/mocap_base/package.xml b/navigation/qualisys_motion_capture/mocap_base/package.xml deleted file mode 100644 index 522872e87..000000000 --- a/navigation/qualisys_motion_capture/mocap_base/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - mocap_base - 0.0.1 - Base class for different motion capture systems - - Ke Sun - Apache-2.0 - Ke Sun - - catkin - geometry_msgs - roscpp - eigen_conversions - - - - diff --git a/navigation/qualisys_motion_capture/mocap_base/src/KalmanFilter.cpp b/navigation/qualisys_motion_capture/mocap_base/src/KalmanFilter.cpp deleted file mode 100644 index 34f081521..000000000 --- a/navigation/qualisys_motion_capture/mocap_base/src/KalmanFilter.cpp +++ /dev/null @@ -1,259 +0,0 @@ -/* - * Copyright [2015] [Ke Sun ] - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include -#include -#include - -using namespace std; -using namespace Eigen; - -namespace mocap { - -KalmanFilter::KalmanFilter(): - attitude (Quaterniond::Identity()), - position (Vector3d::Zero()), - angular_vel (Vector3d::Zero()), - linear_vel (Vector3d::Zero()), - state_cov (Matrix12d::Identity()), - input_cov (Matrix12d::Identity()), - measurement_cov (Matrix6d::Identity()), - filter_status (INIT_POSE), - last_time_stamp (0.0), - msg_interval (0.01), - proc_jacob (Matrix12d::Zero()), - meas_jacob (Matrix6_12d::Zero()), - proc_noise_jacob (Matrix12d::Zero()), - meas_noise_jacob (Matrix6d::Zero()) { - - // Needs to be changed based on angular velocity - // and time interval - proc_jacob = Matrix12d::Identity(); - proc_noise_jacob = Matrix12d::Identity(); - - // Do not need to be changed - meas_jacob.leftCols<6>() = Matrix6d::Identity(); - meas_noise_jacob = Matrix6d::Identity(); - - return; -} - -bool KalmanFilter::init(const Matrix12d& u_cov, - const Matrix6d& m_cov, const int& freq) { - bool is_valid = true; - JacobiSVD u_svd(u_cov); - JacobiSVD m_svd(m_cov); - - Vector12d u_sigmas = u_svd.singularValues(); - Vector6d m_sigmas = m_svd.singularValues(); - - if (u_sigmas(11) < 1e-10) { - is_valid = false; - ROS_ERROR("Input Cov is close to singular (least singlar value:%f < 1e-7)", - u_sigmas(11)); - } else { - input_cov = u_cov; - } - - if (m_sigmas(5) < 1e-10) { - is_valid = false; - ROS_ERROR("Measurement Cov is close to singular (least singlar value:%f < 1e-7)", - m_sigmas(5)); - } else { - measurement_cov = m_cov; - } - - if (freq < 0) { - is_valid = false; - ROS_ERROR("Invalid frequency for filter (%d < 0)", freq); - } else { - msg_interval = 1.0 / static_cast(freq); - } - - return is_valid; -} - -bool KalmanFilter::prepareInitialCondition( - const double& curr_time_stamp, - const Eigen::Quaterniond& m_attitude, - const Eigen::Vector3d& m_position) { - - switch (filter_status) { - case INIT_POSE: { - // Set the current pose - last_time_stamp = curr_time_stamp; - attitude = m_attitude; - position = m_position; - filter_status = INIT_TWIST; - // Set the uncertainty of the state - state_cov = Matrix12d::Identity(); - return true; - } - case INIT_TWIST: { - // Compute the difference between the current - // pose and last pose - Quaterniond dq = m_attitude*attitude.inverse(); - AngleAxisd daa(dq); - Vector3d dr = m_position-position; - double dt = curr_time_stamp-last_time_stamp; - //dt = dt > 0 ? dt : msg_interval; - dt = dt*0.9 + msg_interval*0.1; - // Set current pose and velocity - last_time_stamp += dt; - attitude = m_attitude; - position = m_position; - angular_vel = daa.axis()*daa.angle()/dt; - linear_vel = dr/dt; - // Set the uncertainty of the state - state_cov = Matrix12d::Identity(); - filter_status = READY; - return true; - } - case READY: - return false; - default: - return false; - } - return false; -} - -bool KalmanFilter::isReady() { - if (filter_status != READY) - return false; - else - return true; -} - -void KalmanFilter::reset() { - filter_status = INIT_POSE; - return; -} - -void KalmanFilter::prediction(const double& curr_time_stamp) { - // Propogate the actual state - double dt = curr_time_stamp - last_time_stamp; - last_time_stamp = curr_time_stamp; - - Vector3d dw = angular_vel * dt; - Vector3d dr = linear_vel * dt; - - double dangle = dw.norm(); - Vector3d axis = dw / dangle; - AngleAxisd daa(dangle, axis); - Quaterniond dq(daa); - // Velocities are modeled as constants - attitude = dq * attitude; - position = dr + position; - - // Propogate the uncertainty of the estimation error - // TODO: Optimize this part - proc_jacob(0, 1) = dw(2); - proc_jacob(0, 2) = -dw(1); - proc_jacob(1, 0) = -dw(2); - proc_jacob(1, 2) = dw(0); - proc_jacob(2, 0) = dw(1); - proc_jacob(2, 1) = -dw(0); - proc_jacob(0, 6) = dt; - proc_jacob(1, 7) = dt; - proc_jacob(2, 8) = dt; - proc_jacob(3, 9) = dt; - proc_jacob(4, 10) = dt; - proc_jacob(5, 11) = dt; - - //state_cov = proc_jacob*state_cov*proc_jacob.transpose() + - // proc_noise_jacob*input_cov*proc_noise_jacob.transpose(); - state_cov = proc_jacob*state_cov*proc_jacob.transpose() + input_cov; - - //cout << "Process: " << endl; - //cout << "att: " << - // Vector4d(attitude.w(), attitude.x(), attitude.y(), attitude.z()).transpose() << endl; - //cout << "pos: " << position.transpose() << endl; - //cout << "ang: " << angular_vel.transpose() << endl; - //cout << "lin: " << linear_vel.transpose() << endl; - //cout << "input noise:\n" << input_cov << endl; - //cout << "proc_jacob:\n" << proc_jacob << endl; - //cout << "Pk+1|k:\n" << state_cov << endl; - - return; -} - -void KalmanFilter::update(const Eigen::Quaterniond& m_attitude, - const Eigen::Vector3d& m_position) { - // TODO: Optimize the whole function - // Compute the residual of the measurement - Quaterniond re_q = m_attitude * attitude.inverse(); - AngleAxisd re_aa(re_q); - if (std::abs(re_aa.angle()) > std::abs(2*M_PI-re_aa.angle())) { - re_aa.angle() = 2*M_PI - re_aa.angle(); - re_aa.axis() = -re_aa.axis(); - } - Quaterniond re_qs(re_aa); - Vector3d re_th(re_qs.x()*2.0, re_qs.y()*2.0, re_qs.z()*2.0); - Vector3d re_r = m_position - position; - Vector6d re; - re.head<3>() = re_th; - re.tail<3>() = re_r; - - // Compute the covariance of the residual - //Matrix6d S = measurement_cov + - // meas_jacob*state_cov*meas_jacob.transpose(); - Matrix6d S = measurement_cov + state_cov.topLeftCorner<6, 6>(); - - // Compute the Kalman gain - //Matrix12_6d K = state_cov*meas_jacob.transpose()*S.inverse(); - Matrix12_6d K = state_cov.leftCols<6>()*S.inverse(); - - // Compute the correction of the state - Vector12d dx = K * re; - - // Update the state based on the correction - Vector3d dq_vec3 = dx.head<3>() / 2.0; - Vector4d dq_vec4 = 1.0/sqrt(1.0+dq_vec3.squaredNorm())* - Vector4d(dq_vec3(0), dq_vec3(1), dq_vec3(2), 1.0); - Quaterniond dq(dq_vec4(3), dq_vec4(0), dq_vec4(1), dq_vec4(2)); - - attitude = dq * attitude; - position += dx.segment<3>(3); - angular_vel += dx.segment<3>(6); - linear_vel += dx.segment<3>(9); - - // Update the uncertainty of the state/error - state_cov = (Matrix12d::Identity()-K*meas_jacob) * state_cov; - - //cout << "Error: " << endl; - //cout << "dat: " << Vector4d(re_q.w(), re_q.x(), re_q.y(), re_q.z()).transpose() << endl; - //cout << "dpo: " << re_r.transpose() << endl; - - //cout << "Innovation: " << endl; - //cout << "dat: " << Vector4d(dq.w(), dq.x(), dq.y(), dq.z()).transpose() << endl; - //cout << "dpo: " << dx.segment<3>(3).transpose() << endl; - //cout << "dan: " << dx.segment<3>(6).transpose() << endl; - //cout << "dli: " << dx.segment<3>(9).transpose() << endl; - - //cout << "Update: " << endl; - //cout << "att: " << - // Vector4d(attitude.w(), attitude.x(), attitude.y(), attitude.z()).transpose() << endl; - //cout << "pos: " << position.transpose() << endl; - //cout << "ang: " << angular_vel.transpose() << endl; - //cout << "lin: " << linear_vel.transpose() << endl; - //cout << "means_jacob:\n" << meas_jacob << endl; - //cout << "Pk+1|k:\n" << state_cov << endl; - //cout << "meas_noise:\n" << measurement_cov << endl; - //cout << endl; - - return; -} -} diff --git a/navigation/qualisys_motion_capture/mocap_base/src/MoCapDriverBase.cpp b/navigation/qualisys_motion_capture/mocap_base/src/MoCapDriverBase.cpp deleted file mode 100644 index 3b79f3e7c..000000000 --- a/navigation/qualisys_motion_capture/mocap_base/src/MoCapDriverBase.cpp +++ /dev/null @@ -1,172 +0,0 @@ -/* - * Copyright [2015] [Ke Sun ] - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace Eigen; - -namespace mocap { - -Subject::Subject(ros::NodeHandle* nptr, const string& sub_name, - const std::string& p_frame): - name (sub_name), - status (LOST), - nh_ptr (nptr), - parent_frame (p_frame){ - - pub_filter = nh_ptr->advertise(name+"/odom", 1); - pub_raw = nh_ptr->advertise(name+"/pose", 1); - pub_vel = nh_ptr->advertise(name+"/velocity", 1); - return; -} - -// Get and set name of the subject -const string& Subject::getName() { - boost::shared_lock read_lock(mtx); - return name; -} -void Subject::setName(const string& sub_name) { - boost::unique_lock write_lock(mtx); - name = sub_name; -} - -// Enable or diable the subject -const Subject::Status& Subject::getStatus() { - boost::shared_lock read_lock(mtx); - return status; -} -void Subject::enable() { - boost::unique_lock write_lock(mtx); - status = INITIALIZING; - ROS_INFO("Initializing subject %s", name.c_str()); -} -void Subject::disable() { - boost::unique_lock write_lock(mtx); - kFilter.reset(); - status = LOST; - ROS_WARN("Lost track of subject %s", name.c_str()); -} - -// Get the state of the subject -const Quaterniond& Subject::getAttitude() { - boost::shared_lock read_lock(mtx); - return kFilter.attitude; -} -const Vector3d& Subject::getPosition() { - boost::shared_lock read_lock(mtx); - return kFilter.position; -} -const Vector3d& Subject::getAngularVel() { - boost::shared_lock read_lock(mtx); - return kFilter.angular_vel; -} -const Vector3d& Subject::getLinearVel() { - boost::shared_lock read_lock(mtx); - return kFilter.linear_vel; -} - -// Set the noise parameter for the kalman filter -bool Subject::setParameters( - const Matrix& u_cov, - const Matrix& m_cov, - const int& freq) { - boost::unique_lock write_lock(mtx); - return kFilter.init(u_cov, m_cov, freq); -} - -// Process the new measurement -void Subject::processNewMeasurement( - const double& time, - const Quaterniond& m_attitude, - const Vector3d& m_position) { - //ros::Time tbefore_filter = ros::Time::now(); - - boost::unique_lock write_lock(mtx); - - // Publish raw data from mocap system - geometry_msgs::PoseStamped pose_raw; - pose_raw.header.stamp = ros::Time(time); - pose_raw.header.frame_id = parent_frame; - - tf::quaternionEigenToMsg(m_attitude, pose_raw.pose.orientation); - tf::pointEigenToMsg(m_position, pose_raw.pose.position); - - - pub_raw.publish(pose_raw); - if (!kFilter.isReady()) { - status = INITIALIZING; - kFilter.prepareInitialCondition(time, m_attitude, m_position); - return; - } - - // Transform matrix from parent to child frame - Isometry3d tf_parent2child = Isometry3d(Translation3d(m_position) * m_attitude); - - status = TRACKED; - // Perfrom the kalman filter - - kFilter.prediction(time); - kFilter.update(m_attitude, m_position); - // Publish the new state - nav_msgs::Odometry odom_filter; - odom_filter.header.stamp = ros::Time(time); - odom_filter.header.frame_id = parent_frame; - odom_filter.child_frame_id = name + "/base_link"; - tf::quaternionEigenToMsg(kFilter.attitude, odom_filter.pose.pose.orientation); - tf::pointEigenToMsg(kFilter.position, odom_filter.pose.pose.position); - // Transform twist to child frame - tf::vectorEigenToMsg(tf_parent2child.linear() * kFilter.angular_vel, odom_filter.twist.twist.angular); - tf::vectorEigenToMsg(tf_parent2child.linear() * kFilter.linear_vel, odom_filter.twist.twist.linear); - // To be compatible with the covariance in ROS, we have to do some shifting - Map > pose_cov(odom_filter.pose.covariance.begin()); - Map > vel_cov(odom_filter.twist.covariance.begin()); - pose_cov.topLeftCorner<3, 3>() = kFilter.state_cov.block<3, 3>(3, 3); - pose_cov.topRightCorner<3, 3>() = kFilter.state_cov.block<3, 3>(3, 0); - pose_cov.bottomLeftCorner<3, 3>() = kFilter.state_cov.block<3, 3>(0, 3); - pose_cov.bottomRightCorner<3, 3>() = kFilter.state_cov.block<3, 3>(0, 0); - vel_cov.topLeftCorner<3, 3>() = kFilter.state_cov.block<3, 3>(9, 9); - vel_cov.topRightCorner<3, 3>() = kFilter.state_cov.block<3, 3>(9, 6); - vel_cov.bottomLeftCorner<3, 3>() = kFilter.state_cov.block<3, 3>(6, 9); - vel_cov.bottomRightCorner<3, 3>() = kFilter.state_cov.block<3, 3>(6, 6); - // Transform the velocity cov to child frame - Matrix r_vel = Matrix::Zero(); //Zero initialized velocity cov matrix - r_vel.block<3, 3>(0, 0) = r_vel.block<3, 3>(3, 3) = tf_parent2child.linear(); - vel_cov = r_vel * vel_cov * r_vel.transpose(); - - pub_filter.publish(odom_filter); - //ROS_INFO("filter time: %f", (ros::Time::now() - tbefore_filter).toSec()); - - // Publish velocity in parent frame - geometry_msgs::TwistStamped vel; - vel.header.stamp = ros::Time(time); - vel.header.frame_id = parent_frame; - - tf::vectorEigenToMsg(kFilter.angular_vel, vel.twist.angular); - tf::vectorEigenToMsg(kFilter.linear_vel, vel.twist.linear); - - pub_vel.publish(vel); - - return; -} -} diff --git a/navigation/qualisys_motion_capture/mocap_qualisys/CMakeLists.txt b/navigation/qualisys_motion_capture/mocap_qualisys/CMakeLists.txt deleted file mode 100644 index df941e243..000000000 --- a/navigation/qualisys_motion_capture/mocap_qualisys/CMakeLists.txt +++ /dev/null @@ -1,79 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(mocap_qualisys) - -set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/cmake") - -find_package(Boost REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(catkin REQUIRED COMPONENTS - message_generation - roscpp - geometry_msgs - tf - eigen_conversions - tf_conversions - mocap_base -) - - -catkin_package( - #INCLUDE_DIRS include - #LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS - geometry_msgs tf mocap_base - DEPENDS - Boost EIGEN3 -) - -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} -) - - -if(NOT APPLE) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall") - -# QUALISYS -add_library(mocap_qualisys_driver - src/QualisysDriver.cpp - include/mocap_qualisys/RTProtocol.cpp - include/mocap_qualisys/Markup.cpp - include/mocap_qualisys/RTPacket.cpp - include/mocap_qualisys/Network.cpp -) -target_link_libraries(mocap_qualisys_driver - ${catkin_LIBRARIES} -) -add_dependencies(mocap_qualisys_driver - ${${PROJECT_NAME}_EXPORTED_TARGETS} - - ${catkin_EXPORTED_TARGETS} -) - -add_executable(mocap_qualisys_node - src/qualisys.cpp -) -target_link_libraries(mocap_qualisys_node - mocap_qualisys_driver - ${catkin_LIBRARIES} -) - -install(TARGETS mocap_qualisys_driver mocap_qualisys_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) - -else() - message(ERROR " MacOS is unsupported! mocap_qualisys will not be built!") -endif() - - - diff --git a/navigation/qualisys_motion_capture/mocap_qualisys/include/mocap_qualisys/Markup.cpp b/navigation/qualisys_motion_capture/mocap_qualisys/include/mocap_qualisys/Markup.cpp deleted file mode 100644 index 1e0596d11..000000000 --- a/navigation/qualisys_motion_capture/mocap_qualisys/include/mocap_qualisys/Markup.cpp +++ /dev/null @@ -1,1252 +0,0 @@ -#define _CRT_SECURE_NO_WARNINGS -#define NOMINMAX - -// Markup.cpp: implementation of the NBC_CMarkup class. -// -// NBC_CMarkup Release 6.5 Lite -// Copyright (C) 1999-2003 First Objective Software, Inc. All rights reserved -// This entire notice must be retained in this source code -// Redistributing this source code requires written permission -// This software is provided "as is", with no warranty. -// Latest fixes enhancements and documentation at www.firstobject.com - -#include -#include -#include -#include - -#include "Markup.h" - -#ifdef _DEBUG -#undef THIS_FILE -static char THIS_FILE[]=__FILE__; -#define new DEBUG_NEW -#endif - -void CMarkup::SetIndent(int nIndent) -{ - mnIndent = nIndent; -} - -void CMarkup::operator=(const CMarkup& markup) -{ - m_iPosParent = markup.m_iPosParent; - m_iPos = markup.m_iPos; - m_iPosChild = markup.m_iPosChild; - m_iPosFree = markup.m_iPosFree; - m_nNodeType = markup.m_nNodeType; - m_aPos.clear(); - m_aPos = markup.m_aPos; - m_csDoc = markup.m_csDoc; - MARKUP_SETDEBUGSTATE; -} - -bool CMarkup::SetDoc(const char* szDoc) -{ - // Reset indexes - m_iPosFree = 1; - ResetPos(); - - // Set document text - if (szDoc) - m_csDoc = szDoc; - else - m_csDoc.erase(); - - // Starting size of position array: 1 element per 64 bytes of document - // Tight fit when parsing small doc, only 0 to 2 reallocs when parsing large doc - // Start at 8 when creating new document - std::string::size_type nStartSize = m_csDoc.length() / 64 + 8; - if (m_aPos.size() < nStartSize) - m_aPos.resize(nStartSize); - - // Parse document - bool bWellFormed = false; - if (m_csDoc.length()) - { - m_aPos[0].Clear(); - int iPos = x_ParseElem(0); - if (iPos > 0) - { - m_aPos[0].iElemChild = iPos; - bWellFormed = true; - } - } - - // Clear indexes if parse failed or empty document - if (! bWellFormed) - { - m_aPos[0].Clear(); - m_iPosFree = 1; - } - - ResetPos(); - - memset(mtIndent, ' ', sizeof(mtIndent)); - mtIndent[999] = 0; - return bWellFormed; -} - -bool CMarkup::IsWellFormed() -{ - if (!(m_aPos.empty()) && m_aPos[0].iElemChild) - return true; - return false; -} - -bool CMarkup::FindElem(const char* szName) -{ - // Change current position only if found - // - if (!m_aPos.empty()) - { - int iPos = x_FindElem(m_iPosParent, m_iPos, szName); - if (iPos) - { - // Assign new position - x_SetPos(m_aPos[iPos].iElemParent, iPos, 0); - return true; - } - } - return false; -} - -bool CMarkup::FindChildElem(const char* szName) -{ - // Change current child position only if found - // - // Shorthand: call this with no current main position - // means find child under root element - if (! m_iPos) - FindElem(); - - int iPosChild = x_FindElem(m_iPos, m_iPosChild, szName); - if (iPosChild) - { - // Assign new position - int iPos = m_aPos[iPosChild].iElemParent; - x_SetPos(m_aPos[iPos].iElemParent, iPos, iPosChild); - return true; - } - - return false; -} - - -std::string CMarkup::GetTagName() const -{ - // Return the tag name at the current main position - std::string csTagName; - - - if (m_iPos) - csTagName = x_GetTagName(m_iPos); - return csTagName; -} - -bool CMarkup::IntoElem() -{ - // If there is no child position and IntoElem is called it will succeed in release 6.3 - // (A subsequent call to FindElem will find the first element) - // The following short-hand behavior was never part of EDOM and was misleading - // It would find a child element if there was no current child element position and go into it - // It is removed in release 6.3, this change is NOT backwards compatible! - // if (! m_iPosChild) - // FindChildElem(); - - if (m_iPos && m_nNodeType == MNT_ELEMENT) - { - x_SetPos(m_iPos, m_iPosChild, 0); - return true; - } - return false; -} - -bool CMarkup::OutOfElem() -{ - // Go to parent element - if (m_iPosParent) - { - x_SetPos(m_aPos[m_iPosParent].iElemParent, m_iPosParent, m_iPos); - return true; - } - return false; -} - -////////////////////////////////////////////////////////////////////// -// Private Methods -////////////////////////////////////////////////////////////////////// - -int CMarkup::x_GetFreePos() -{ - // - // This returns the index of the next unused ElemPos in the array - // - if (m_iPosFree == (int)m_aPos.size()) - m_aPos.resize(m_iPosFree + m_iPosFree / 2); - ++m_iPosFree; - return m_iPosFree - 1; -} - -int CMarkup::x_ReleasePos() -{ - // - // This decrements the index of the next unused ElemPos in the array - // allowing the element index returned by GetFreePos() to be reused - // - --m_iPosFree; - return 0; -} - -int CMarkup::x_ParseError(const char* szError, const char* szName) -{ - if (szName) - m_csError = Format(szError, szName); - else - m_csError = szError; - x_ReleasePos(); - return -1; -} - -int CMarkup::x_ParseElem(int iPosParent) -{ - // This is either called by SetDoc, x_AddSubDoc, or itself recursively - // m_aPos[iPosParent].nEndL is where to start parsing for the child element - // This returns the new position if a tag is found, otherwise zero - // In all cases we need to get a new ElemPos, but release it if unused - // - int iPos = x_GetFreePos(); - m_aPos[iPos].nStartL = m_aPos[iPosParent].nEndL; - m_aPos[iPos].iElemParent = iPosParent; - m_aPos[iPos].iElemChild = 0; - m_aPos[iPos].iElemNext = 0; - - // Start Tag - // A loop is used to ignore all remarks tags and special tags - // i.e. , and - // So any tag beginning with ? or ! is ignored - // Loop past ignored tags - TokenPos token(m_csDoc.c_str()); - token.nNext = m_aPos[iPosParent].nEndL; - std::string csName; - while (csName.empty()) - { - // Look for left angle bracket of start tag - m_aPos[iPos].nStartL = token.nNext; - if (! x_FindChar(token.szDoc, m_aPos[iPos].nStartL, '<')) - return x_ParseError("Element tag not found"); - - // Set parent's End tag to start looking from here (or later) - m_aPos[iPosParent].nEndL = m_aPos[iPos].nStartL; - - // Determine whether this is an element, or bypass other type of node - token.nNext = m_aPos[iPos].nStartL + 1; - if (x_FindToken(token)) - { - if (token.bIsString) - return x_ParseError("Tag starts with quote"); - char cFirstChar = m_csDoc[token.nL]; - if (cFirstChar == '?' || cFirstChar == '!') - { - token.nNext = m_aPos[iPos].nStartL; - if (! x_ParseNode(token)) - return x_ParseError("Invalid node"); - } - else if (cFirstChar != '/') - { - csName = x_GetToken(token); - // Look for end of tag - if (! x_FindChar(token.szDoc, token.nNext, '>')) - return x_ParseError("End of tag not found"); - } - else - return x_ReleasePos(); // probably end tag of parent - } - else - return x_ParseError("Abrupt end within tag"); - } - m_aPos[iPos].nStartR = token.nNext; - - // Is ending mark within start tag, i.e. empty element? - if (m_csDoc[m_aPos[iPos].nStartR-1] == '/') - { - // Empty element - // Close tag left is set to ending mark, and right to open tag right - m_aPos[iPos].nEndL = m_aPos[iPos].nStartR-1; - m_aPos[iPos].nEndR = m_aPos[iPos].nStartR; - } - else // look for end tag - { - // Element probably has contents - // Determine where to start looking for left angle bracket of end tag - // This is done by recursively parsing the contents of this element - int iInner, iInnerPrev = 0; - m_aPos[iPos].nEndL = m_aPos[iPos].nStartR + 1; - while ((iInner = x_ParseElem(iPos)) > 0) - { - // Set links to iInner - if (iInnerPrev) - m_aPos[iInnerPrev].iElemNext = iInner; - else - m_aPos[iPos].iElemChild = iInner; - iInnerPrev = iInner; - - // Set offset to reflect child - m_aPos[iPos].nEndL = m_aPos[iInner].nEndR + 1; - } - if (iInner == -1) - return -1; - - // Look for left angle bracket of end tag - if (! x_FindChar(token.szDoc, m_aPos[iPos].nEndL, '<')) - return x_ParseError("End tag of %s element not found", csName.c_str()); - - // Look through tokens of end tag - token.nNext = m_aPos[iPos].nEndL + 1; - int nTokenCount = 0; - while (x_FindToken(token)) - { - ++nTokenCount; - if (! token.bIsString) - { - // Is first token not an end slash mark? - if (nTokenCount == 1 && m_csDoc[token.nL] != '/') - return x_ParseError("Expecting end tag of element %s", csName.c_str()); - - else if (nTokenCount == 2 && ! token.Match(csName.c_str())) - return x_ParseError("End tag does not correspond to %s", csName.c_str()); - - // Else is it a right angle bracket? - else if (m_csDoc[token.nL] == '>') - break; - } - } - - // Was a right angle bracket not found? - if (! token.szDoc[token.nL] || nTokenCount < 2) - return x_ParseError("End tag not completed for element %s", csName.c_str()); - m_aPos[iPos].nEndR = token.nL; - } - - // Successfully parsed element (and contained elements) - return iPos; -} - -bool CMarkup::x_FindChar(const char* szDoc, int& nChar, char c) -{ - // static function - const char* pChar = &szDoc[nChar]; - while (*pChar && *pChar != c) - pChar += 1; //_tclen(pChar); - nChar = (int)(pChar - szDoc); - if (! *pChar) - return false; - /* - while (szDoc[nChar] && szDoc[nChar] != c) - nChar += _tclen(&szDoc[nChar]); - if (! szDoc[nChar]) - return false; - */ - return true; -} - -bool CMarkup::x_FindAny(const char* szDoc, int& nChar) -{ - // Starting at nChar, find a non-whitespace char - // return false if no non-whitespace before end of document, nChar points to end - // otherwise return true and nChar points to non-whitespace char - while (szDoc[nChar] && strchr(" \t\n\r", szDoc[nChar])) - ++nChar; - return szDoc[nChar] != '\0'; -} - -bool CMarkup::x_FindToken(CMarkup::TokenPos& token) -{ - // Starting at token.nNext, bypass whitespace and find the next token - // returns true on success, members of token point to token - // returns false on end of document, members point to end of document - const char* szDoc = token.szDoc; - int nChar = token.nNext; - token.bIsString = false; - - // By-pass leading whitespace - if (! x_FindAny(szDoc,nChar)) - { - // No token was found before end of document - token.nL = nChar; - token.nR = nChar; - token.nNext = nChar; - return false; - } - - // Is it an opening quote? - char cFirstChar = szDoc[nChar]; - if (cFirstChar == '\"' || cFirstChar == '\'') - { - token.bIsString = true; - - // Move past opening quote - ++nChar; - token.nL = nChar; - - // Look for closing quote - x_FindChar(token.szDoc, nChar, cFirstChar); - - // Set right to before closing quote - token.nR = nChar - 1; - - // Set nChar past closing quote unless at end of document - if (szDoc[nChar]) - ++nChar; - } - else - { - // Go until special char or whitespace - token.nL = nChar; - while (szDoc[nChar] && ! strchr(" \t\n\r<>=\\/?!", szDoc[nChar])) - nChar += 1; //_tclen(&szDoc[nChar]); - - // Adjust end position if it is one special char - if (nChar == token.nL) - ++nChar; // it is a special char - token.nR = nChar - 1; - } - - // nNext points to one past last char of token - token.nNext = nChar; - return true; -} - -std::string CMarkup::x_GetToken(const CMarkup::TokenPos& token) const -{ - // The token contains indexes into the document identifying a small substring - // Build the substring from those indexes and return it - if (token.nL > token.nR) - return ""; - return Mid(m_csDoc, token.nL, - token.nR - token.nL + ((token.nR < (int)(m_csDoc.length())) ? 1 : 0)); -} - -int CMarkup::x_FindElem(int iPosParent, int iPos, const char* szPath) -{ - // If szPath is NULL or empty, go to next sibling element - // Otherwise go to next sibling element with matching path - // - if (iPos) - iPos = m_aPos[iPos].iElemNext; - else - iPos = m_aPos[iPosParent].iElemChild; - - // Finished here if szPath not specified - if (szPath == NULL || !szPath[0]) - return iPos; - - // Search - TokenPos token(m_csDoc.c_str()); - while (iPos) - { - // Compare tag name - token.nNext = m_aPos[iPos].nStartL + 1; - x_FindToken(token); // Locate tag name - if (token.Match(szPath)) - return iPos; - iPos = m_aPos[iPos].iElemNext; - } - return 0; -} - -int CMarkup::x_ParseNode(CMarkup::TokenPos& token) -{ - // Call this with token.nNext set to the start of the node - // This returns the node type and token.nNext set to the char after the node - // If the node is not found or an element, token.nR is not determined - int nTypeFound = 0; - const char* szDoc = token.szDoc; - token.nL = token.nNext; - if (szDoc[token.nL] == '<') - { - // Started with <, could be: - // comment - // dtd - // processing instruction - // cdata section - // element - // - if (! szDoc[token.nL+1] || ! szDoc[token.nL+2]) - return 0; - char cFirstChar = szDoc[token.nL+1]; - const char* szEndOfNode = NULL; - if (cFirstChar == '?') - { - nTypeFound = MNT_PROCESSING_INSTRUCTION; - szEndOfNode = "?>"; - } - else if (cFirstChar == '!') - { - char cSecondChar = szDoc[token.nL+2]; - if (cSecondChar == '[') - { - nTypeFound = MNT_CDATA_SECTION; - szEndOfNode = "]]>"; - } - else if (cSecondChar == '-') - { - nTypeFound = MNT_COMMENT; - szEndOfNode = "-->"; - } - else - { - // Document type requires tokenizing because of strings and brackets - nTypeFound = 0; - int nBrackets = 0; - while (x_FindToken(token)) - { - if (! token.bIsString) - { - char cChar = szDoc[token.nL]; - if (cChar == '[') - ++nBrackets; - else if (cChar == ']') - --nBrackets; - else if (nBrackets == 0 && cChar == '>') - { - nTypeFound = MNT_DOCUMENT_TYPE; - break; - } - } - } - if (! nTypeFound) - return 0; - } - } - else if (cFirstChar == '/') - { - // End tag means no node found within parent element - return 0; - } - else - { - nTypeFound = MNT_ELEMENT; - } - - // Search for end of node if not found yet - if (szEndOfNode) - { - const char* pEnd = strstr(&szDoc[token.nNext], szEndOfNode); - if (! pEnd) - return 0; // not well-formed - token.nNext = (int)(pEnd - szDoc) + (int)strlen(szEndOfNode); - } - } - else if (szDoc[token.nL]) - { - // It is text or whitespace because it did not start with < - nTypeFound = MNT_WHITESPACE; - token.nNext = token.nL; - if (x_FindAny(szDoc,token.nNext)) - { - if (szDoc[token.nNext] != '<') - { - nTypeFound = MNT_TEXT; - x_FindChar(szDoc, token.nNext, '<'); - } - } - } - return nTypeFound; -} - -std::string CMarkup::x_GetTagName(int iPos) const -{ - // Return the tag name at specified element - TokenPos token(m_csDoc.c_str()); - token.nNext = m_aPos[iPos].nStartL + 1; - if (! iPos || ! x_FindToken(token)) - return ""; - - // Return substring of document - return x_GetToken(token); -} - -bool CMarkup::x_FindAttrib(CMarkup::TokenPos& token, const char* szAttrib) const -{ - // If szAttrib is NULL find next attrib, otherwise find named attrib - // Return true if found - int nAttrib = 0; - for (int nCount = 0; x_FindToken(token); ++nCount) - { - if (! token.bIsString) - { - // Is it the right angle bracket? - char cChar = m_csDoc[token.nL]; - if (cChar == '>' || cChar == '/' || cChar == '?') - break; // attrib not found - - // Equal sign - if (cChar == '=') - continue; - - // Potential attribute - if (! nAttrib && nCount) - { - // Attribute name search? - if (! szAttrib || ! szAttrib[0]) - return true; // return with token at attrib name - - // Compare szAttrib - if (token.Match(szAttrib)) - nAttrib = nCount; - } - } - else if (nAttrib && nCount == nAttrib + 2) - { - return true; - } - } - - // Not found - return false; -} - -std::string CMarkup::x_GetAttrib(int iPos, const char* szAttrib) const -{ - // Return the value of the attrib - TokenPos token(m_csDoc.c_str()); - if (iPos && m_nNodeType == MNT_ELEMENT) - token.nNext = m_aPos[iPos].nStartL + 1; - else - return ""; - - if (szAttrib && x_FindAttrib(token, szAttrib)) - return x_TextFromDoc(token.nL, token.nR - ((token.nR < (int)(m_csDoc.length())) ? 0 : 1)); - return ""; -} - -bool CMarkup::x_SetAttrib(int iPos, const char* szAttrib, const char* szValue) -{ - // Set attribute in iPos element - TokenPos token(m_csDoc.c_str()); - int nInsertAt; - if (iPos && m_nNodeType == MNT_ELEMENT) - { - token.nNext = m_aPos[iPos].nStartL + 1; - nInsertAt = m_aPos[iPos].nStartR - (m_aPos[iPos].IsEmptyElement()?1:0); - } - else - return false; - - // Create insertion text depending on whether attribute already exists - int nReplace = 0; - std::string csInsert; - if (x_FindAttrib(token, szAttrib)) - { - // Replace value only - // Decision: for empty value leaving attrib="" instead of removing attrib - csInsert = x_TextToDoc(szValue, true); - nInsertAt = token.nL; - nReplace = token.nR-token.nL+1; - } - else - { - // Insert string name value pair - std::string csFormat; - csFormat = " "; - csFormat += szAttrib; - csFormat += "=\""; - csFormat += x_TextToDoc(szValue, true); - csFormat += "\""; - csInsert = csFormat; - } - - x_DocChange(nInsertAt, nReplace, csInsert); - int nAdjust = (int)csInsert.length() - nReplace; - m_aPos[iPos].nStartR += nAdjust; - m_aPos[iPos].AdjustEnd(nAdjust); - x_Adjust(iPos, nAdjust); - MARKUP_SETDEBUGSTATE; - return true; -} - -std::string CMarkup::x_GetData(int iPos) const -{ - - // Return a string representing data between start and end tag - // Return empty string if there are any children elements - if (! m_aPos[iPos].iElemChild && ! m_aPos[iPos].IsEmptyElement()) - { - // See if it is a CDATA section - const char* szDoc = (const char*)(m_csDoc.c_str()); - int nChar = m_aPos[iPos].nStartR + 1; - if (x_FindAny(szDoc, nChar) && szDoc[nChar] == '<' - && nChar + 11 < m_aPos[iPos].nEndL - && strncmp(&szDoc[nChar], "", nChar); - if (nEndCDATA != -1 && nEndCDATA < m_aPos[iPos].nEndL) - { - return Mid(m_csDoc, nChar, nEndCDATA - nChar); - } - } - return x_TextFromDoc(m_aPos[iPos].nStartR+1, m_aPos[iPos].nEndL-1); - } - return ""; -} - -std::string CMarkup::x_TextToDoc(const char* szText, bool bAttrib) const -{ - // Convert text as seen outside XML document to XML friendly - // replacing special characters with ampersand escape codes - // E.g. convert "6>7" to "6>7" - // - // < less than - // & ampersand - // > greater than - // - // and for attributes: - // - // ' apostrophe or single quote - // " double quote - // - static const char* szaReplace[] = { "<", "&", ">", "'", """ }; - const char* pFind = bAttrib ? "<&>\'\"" : "<&>"; - std::string csText; - const char* pSource = szText; - int nDestSize = (int)strlen(pSource); - nDestSize += nDestSize / 10 + 7; - char* pDest = GetBuffer(csText, nDestSize); - int nLen = 0; - char cSource = *pSource; - const char* pFound; - while (cSource) - { - if (nLen > nDestSize - 6) - { - ReleaseBuffer(csText, nLen); - nDestSize *= 2; - pDest = GetBuffer(csText, nDestSize); - } - if ((pFound = strchr(pFind,cSource)) != NULL) - { - pFound = szaReplace[pFound-pFind]; -#ifdef _WIN32 - strcpy_s(&pDest[nLen], nDestSize, pFound); -#else - strncpy(&pDest[nLen], pFound, nDestSize); -#endif - nLen += (int)strlen(pFound); - } - else - { - pDest[nLen] = *pSource; - nLen += 1; //_tclen(pSource); - } - pSource += 1; //_tclen(pSource); - cSource = *pSource; - } - ReleaseBuffer(csText, nLen); - return csText; -} - -std::string CMarkup::x_TextFromDoc(int nLeft, int nRight) const -{ - // Convert XML friendly text to text as seen outside XML document - // ampersand escape codes replaced with special characters e.g. convert "6>7" to "6>7" - // Conveniently the result is always the same or shorter in byte length - // - static const char* szaCode[] = { "lt;", "amp;", "gt;", "apos;", "quot;" }; - static int anCodeLen[] = { 3,4,3,5,5 }; - static const char* szSymbol = "<&>\'\""; - std::string csText; - const char* pSource = m_csDoc.c_str(); - int nDestSize = nRight - nLeft + 1; - char* pDest = GetBuffer(csText, nDestSize); - int nLen = 0; - int nCharLen; - int nChar = nLeft; - while (nChar <= nRight) - { - if (pSource[nChar] == '&') - { - // Look for matching &code; - bool bCodeConverted = false; - for (int nMatch = 0; nMatch < 5; ++nMatch) - { - if (nChar <= nRight - anCodeLen[nMatch] - && strncmp(szaCode[nMatch],&pSource[nChar+1],anCodeLen[nMatch]) == 0) - { - // Insert symbol and increment index past ampersand semi-colon - pDest[nLen++] = szSymbol[nMatch]; - nChar += anCodeLen[nMatch] + 1; - bCodeConverted = true; - break; - } - } - - // If the code is not converted, leave it as is - if (! bCodeConverted) - { - pDest[nLen++] = '&'; - ++nChar; - } - } - else // not & - { - nCharLen = 1; //_tclen(&pSource[nChar]); - pDest[nLen] = pSource[nChar]; - nLen += nCharLen; - nChar += nCharLen; - } - } - ReleaseBuffer(csText, nLen); - return csText; -} - -void CMarkup::x_DocChange(int nLeft, int nReplace, const std::string& csInsert) -{ - // Insert csInsert int m_csDoc at nLeft replacing nReplace chars - // Do this with only one buffer reallocation if it grows - // - int nDocLength = (int)m_csDoc.length(); - int nInsLength = (int)csInsert.length(); - - // Make sure nLeft and nReplace are within bounds - nLeft = std::max(0, std::min(nLeft, nDocLength)); - nReplace = std::max(0, std::min(nReplace, nDocLength-nLeft)); - - // Get pointer to buffer with enough room - int nNewLength = nInsLength + nDocLength - nReplace; - int nBufferLen = nNewLength; - char* pDoc = GetBuffer(m_csDoc, nBufferLen); - - // Move part of old doc that goes after insert - if (nLeft+nReplace < nDocLength) - memmove(&pDoc[nLeft+nInsLength], &pDoc[nLeft+nReplace], (nDocLength-nLeft-nReplace)*sizeof(char)); - - // Copy insert - memcpy(&pDoc[nLeft], csInsert.c_str(), nInsLength*sizeof(char)); - - // Release - ReleaseBuffer(m_csDoc, nNewLength); -} - -void CMarkup::x_Adjust(int iPos, int nShift, bool bAfterPos) -{ - // Loop through affected elements and adjust indexes - // Algorithm: - // 1. update children unless bAfterPos - // (if no children or bAfterPos is true, end tag of iPos not affected) - // 2. update next siblings and their children - // 3. go up until there is a next sibling of a parent and update end tags - // 4. step 2 - int iPosTop = m_aPos[iPos].iElemParent; - bool bPosFirst = bAfterPos; // mark as first to skip its children - while (iPos) - { - // Were we at containing parent of affected position? - bool bPosTop = false; - if (iPos == iPosTop) - { - // Move iPosTop up one towards root - iPosTop = m_aPos[iPos].iElemParent; - bPosTop = true; - } - - // Traverse to the next update position - if (! bPosTop && ! bPosFirst && m_aPos[iPos].iElemChild) - { - // Depth first - iPos = m_aPos[iPos].iElemChild; - } - else if (m_aPos[iPos].iElemNext) - { - iPos = m_aPos[iPos].iElemNext; - } - else - { - // Look for next sibling of a parent of iPos - // When going back up, parents have already been done except iPosTop - while ((iPos=m_aPos[iPos].iElemParent) != 0 && iPos != iPosTop) - if (m_aPos[iPos].iElemNext) - { - iPos = m_aPos[iPos].iElemNext; - break; - } - } - bPosFirst = false; - - // Shift indexes at iPos - if (iPos != iPosTop) - m_aPos[iPos].AdjustStart(nShift); - m_aPos[iPos].AdjustEnd(nShift); - } -} - -void CMarkup::x_LocateNew(int iPosParent, int& iPosRel, int& nOffset, int nLength, int nFlags) -{ - // Determine where to insert new element or node - // - bool bInsert = (nFlags&1)?true:false; - bool bHonorWhitespace = (nFlags&2)?true:false; - - std::string::size_type nStartL; - if (nLength) - { - // Located at a non-element node - if (bInsert) - nStartL = nOffset; - else - nStartL = nOffset + nLength; - } - else if (iPosRel) - { - // Located at an element - if (bInsert) // precede iPosRel - nStartL = m_aPos[iPosRel].nStartL; - else // follow iPosRel - nStartL = m_aPos[iPosRel].nEndR + 1; - } - else if (! iPosParent) - { - // Outside of all elements - if (bInsert) - nStartL = 0; - else - nStartL = m_csDoc.length(); - } - else if (m_aPos[iPosParent].IsEmptyElement()) - { - // Parent has no separate end tag, so split empty element - nStartL = m_aPos[iPosParent].nStartR; - } - else - { - if (bInsert) // after start tag - nStartL = m_aPos[iPosParent].nStartR + 1; - else // before end tag - nStartL = m_aPos[iPosParent].nEndL; - } - - // Go up to start of next node, unless its splitting an empty element - if (! bHonorWhitespace && ! m_aPos[iPosParent].IsEmptyElement()) - { - const char* szDoc = (const char*)m_csDoc.c_str(); - int nChar = (int)nStartL; - if (! x_FindAny(szDoc,nChar) || szDoc[nChar] == '<') - nStartL = nChar; - } - - // Determine iPosBefore - int iPosBefore = 0; - if (iPosRel) - { - if (bInsert) - { - // Is iPosRel past first sibling? - int iPosPrev = m_aPos[iPosParent].iElemChild; - if (iPosPrev != iPosRel) - { - // Find previous sibling of iPosRel - while (m_aPos[iPosPrev].iElemNext != iPosRel) - iPosPrev = m_aPos[iPosPrev].iElemNext; - iPosBefore = iPosPrev; - } - } - else - { - iPosBefore = iPosRel; - } - } - else if (m_aPos[iPosParent].iElemChild) - { - if (! bInsert) - { - // Find last element under iPosParent - int iPosLast = m_aPos[iPosParent].iElemChild; - int iPosNext = iPosLast; - while (iPosNext) - { - iPosLast = iPosNext; - iPosNext = m_aPos[iPosNext].iElemNext; - } - iPosBefore = iPosLast; - } - } - - nOffset = (int)nStartL; - iPosRel = iPosBefore; -} - -bool CMarkup::x_AddElem(const char* szName, const char* szValue, bool bInsert, bool bAddChild) -{ - if (bAddChild) - { - // Adding a child element under main position - if (! m_iPos) - return false; - } - else if (m_iPosParent == 0) - { - // Adding root element - if (IsWellFormed()) - return false; - - - // Locate after any version and DTD - m_aPos[0].nEndL = (int)m_csDoc.length(); - } - - // Locate where to add element relative to current node - int iPosParent, iPosBefore, nOffset = 0, nLength = 0; - if (bAddChild) - { - iPosParent = m_iPos; - iPosBefore = m_iPosChild; - } - else - { - iPosParent = m_iPosParent; - iPosBefore = m_iPos; - } - int nFlags = bInsert?1:0; - x_LocateNew(iPosParent, iPosBefore, nOffset, nLength, nFlags); - // LocateNew: in case of an empty parent it finds the end of the start tag (sort of) - // in case of a non-empty parent it finds the char before the start of the end tag. - - - // Find out the indent we need: - int nTopParent = iPosParent; - int nLevel = 0; - while (nTopParent) - { - nTopParent = m_aPos[nTopParent].iElemParent; - nLevel++; - } - int nIndentChars = nLevel * mnIndent; - mtIndent[ nIndentChars ] = 0; - - - bool bEmptyParent = m_aPos[iPosParent].IsEmptyElement(); - if (bEmptyParent || m_aPos[iPosParent].nStartR + 1 == m_aPos[iPosParent].nEndL) - { - nOffset += 2; - } - else - { - if ((nOffset < (int)(m_csDoc.length())) && (0 < nOffset) && - (' ' == m_csDoc[nOffset-1])) - { - while ((0 < nOffset) && (' ' == m_csDoc[nOffset-1])) - --nOffset; - } - } - - // Create element and modify positions of affected elements - // If no szValue is specified, an empty element is created - // i.e. either value or - int iPos = x_GetFreePos(); - m_aPos[iPos].nStartL = nOffset + nIndentChars; - - // Set links - m_aPos[iPos].iElemParent = iPosParent; - m_aPos[iPos].iElemChild = 0; - m_aPos[iPos].iElemNext = 0; - if (iPosBefore) - { - // Link in after iPosBefore - m_aPos[iPos].iElemNext = m_aPos[iPosBefore].iElemNext; - m_aPos[iPosBefore].iElemNext = iPos; - } - else - { - // First child - m_aPos[iPos].iElemNext = m_aPos[iPosParent].iElemChild; - m_aPos[iPosParent].iElemChild = iPos; - } - - // Create string for insert - std::string csInsert; - int nLenName = (int)strlen(szName); - int nLenValue = szValue ? (int)strlen(szValue) : 0; - if (! nLenValue) - { - // empty element - csInsert = mtIndent; - csInsert += "<"; - csInsert += szName; - csInsert += "/>\r\n"; - m_aPos[iPos].nStartR = m_aPos[iPos].nStartL + nLenName + 2; - m_aPos[iPos].nEndL = m_aPos[iPos].nStartR - 1; - m_aPos[iPos].nEndR = m_aPos[iPos].nEndL + 1; - } - else - { - // value - std::string csValue = x_TextToDoc(szValue); - nLenValue = (int)csValue.length(); - csInsert = mtIndent; - csInsert += "<"; - csInsert += szName; - csInsert += ">"; - csInsert += csValue; - csInsert += "\r\n"; - m_aPos[iPos].nStartR = m_aPos[iPos].nStartL + nLenName + 1; - m_aPos[iPos].nEndL = m_aPos[iPos].nStartR + nLenValue + 1; - m_aPos[iPos].nEndR = m_aPos[iPos].nEndL + nLenName + 2; - } - mtIndent[ nIndentChars ] = ' '; - - - - - // Insert - int nReplace = 0, nLeft = m_aPos[iPos].nStartL; - if (bEmptyParent) - { - std::string csParentTagName = x_GetTagName(iPosParent); - std::string csFormat; - csFormat = ">\r\n"; - csFormat += csInsert; - mtIndent[ nIndentChars - mnIndent ] = 0; - csFormat += mtIndent; - mtIndent[ nIndentChars - mnIndent ] = ' '; - csFormat += " (len 4) becomes (len 11) - // In x_Adjust everything will be adjusted 11 - 4 = 7 - // But the nEndL of element A should only be adjusted 5 - m_aPos[iPosParent].nEndL -= (int)(csParentTagName.length() + 1); - } - else if (m_aPos[iPosParent].nStartR + 1 == m_aPos[iPosParent].nEndL) - { - // Empty parent, but with an end tag following right after. - csInsert = "\r\n" + csInsert; - mtIndent[ nIndentChars - mnIndent ] = 0; - csInsert += mtIndent; - mtIndent[ nIndentChars - mnIndent ] = ' '; - nLeft = m_aPos[iPosParent].nStartR + 1; - } - else - { - nLeft -= nIndentChars; - } - - x_DocChange(nLeft, nReplace, csInsert); - x_Adjust(iPos, (int)csInsert.length() - nReplace); - - if (bAddChild) - x_SetPos(m_iPosParent, iPosParent, iPos); - else - x_SetPos(iPosParent, iPos, 0); - return true; -} - -#ifndef _WIN32 -int _vscprintf(const char* format, va_list pargs) -{ - int retval; - va_list argcopy; - va_copy(argcopy, pargs); - retval = vsnprintf(NULL, 0, format, argcopy); - va_end(argcopy); - return retval; -} -#endif - -std::string CMarkup::Format(const char *fmt, ...) -{ - std::string retStr; - if (NULL != fmt) - { - va_list marker; - - // initialize variable arguments - va_start(marker, fmt); - - // Get formatted string length adding one for NULL - int len = _vscprintf(fmt, marker) + 1; - - // Create a char vector to hold the formatted string. - std::vector buffer(len, '\0'); -#ifdef _WIN32 - int nWritten = _vsnprintf_s(&buffer[0], buffer.size(), len, fmt, marker); -#else - int nWritten = vsnprintf(&buffer[0], len, fmt, marker); -#endif - if (nWritten > 0) - { - retStr = &buffer[0]; - } - - // Reset variable arguments - va_end(marker); - } - return retStr; -} - -std::string CMarkup::Mid(const std::string &tStr, int nFirst) const -{ - return Mid(tStr, nFirst, (int)tStr.length() - nFirst); -} - -std::string CMarkup::Mid(const std::string &tStr, int nFirst, int nCount) const -{ - if (nFirst < 0) - { - nFirst = 0; - } - if (nCount < 0) - { - nCount = 0; - } - - int nSize = static_cast(tStr.size()); - - if (nFirst + nCount > nSize) - { - nCount = nSize - nFirst; - } - - if (nFirst > nSize) - { - std::string tStrEmpty; - return tStrEmpty; - } - - assert(nFirst >= 0); - assert(nFirst + nCount <= nSize); - - return tStr.substr(nFirst, nCount); -} - -char* CMarkup::GetBuffer(std::string &tStr, int nMinLen) const -{ - if (static_cast(tStr.size()) < nMinLen) - { - tStr.resize(nMinLen); - } - - return const_cast(tStr.c_str()); //tStr.empty() ? const_cast(tStr.c_str()) : &(tStr.at(0)); -} - -void CMarkup::ReleaseBuffer(std::string &tStr, int nNewLen) const -{ - tStr.resize(nNewLen > -1 ? nNewLen : strlen(tStr.c_str())); -} - -bool CMarkup::TokenPos::Match(const char* szName) const -{ - int nLen = nR - nL + 1; - return ((strncmp(&szDoc[nL], szName, nLen) == 0) - && (szName[nLen] == '\0' || strchr(" =/[", szName[nLen]))); -} diff --git a/navigation/qualisys_motion_capture/mocap_qualisys/include/mocap_qualisys/Markup.h b/navigation/qualisys_motion_capture/mocap_qualisys/include/mocap_qualisys/Markup.h deleted file mode 100644 index 397ee7920..000000000 --- a/navigation/qualisys_motion_capture/mocap_qualisys/include/mocap_qualisys/Markup.h +++ /dev/null @@ -1,170 +0,0 @@ -#ifndef QUALISYS_NBC_NBC_MARKUP_H_INCLUDED -#define QUALISYS_NBC_NBC_MARKUP_H_INCLUDED - -// Markup.h: interface for the NBC_CMarkup class. -// -// NBC_CMarkup Release 6.5 Lite -// Copyright (C) 1999-2003 First Objective Software, Inc. All rights reserved -// This entire notice must be retained in this source code -// Redistributing this source code requires written permission -// This software is provided "as is", with no warranty. -// Latest fixes enhancements and documentation at www.firstobject.com - -#include -#include - -#ifdef _DEBUG -#define _DS(i) (i?&((const char*)m_csDoc.c_str())[m_aPos[i].nStartL]:0) -#define MARKUP_SETDEBUGSTATE m_pMainDS=_DS(m_iPos); m_pChildDS=_DS(m_iPosChild) -#else -#define MARKUP_SETDEBUGSTATE -#endif - -class CMarkup -{ -public: - CMarkup() { SetDoc(NULL); mnIndent = 4; }; - CMarkup(const char* szDoc) { SetDoc(szDoc); }; - CMarkup(const CMarkup& markup) { *this = markup; }; - void operator=(const CMarkup& markup); - virtual ~CMarkup() {}; - - // Settings - void SetIndent(int nIndent = 4); - - // Create - std::string GetDoc() const { return m_csDoc; }; - bool AddElem(const char* szName, const char* szData=NULL) { return x_AddElem(szName,szData,false,false); }; - bool AddChildElem(const char* szName, const char* szData=NULL) { return x_AddElem(szName,szData,false,true); }; - bool AddAttrib(const char* szAttrib, const char* szValue) { return x_SetAttrib(m_iPos,szAttrib,szValue); }; - bool AddChildAttrib(const char* szAttrib, const char* szValue) { return x_SetAttrib(m_iPosChild,szAttrib,szValue); }; - bool SetAttrib(const char* szAttrib, const char* szValue) { return x_SetAttrib(m_iPos,szAttrib,szValue); }; - bool SetChildAttrib(const char* szAttrib, const char* szValue) { return x_SetAttrib(m_iPosChild,szAttrib,szValue); }; - - // Navigate - bool SetDoc(const char* szDoc); - bool IsWellFormed(); - bool FindElem(const char* szName=NULL); - bool FindChildElem(const char* szName=NULL); - bool IntoElem(); - bool OutOfElem(); - void ResetChildPos() { x_SetPos(m_iPosParent,m_iPos,0); }; - void ResetMainPos() { x_SetPos(m_iPosParent,0,0); }; - void ResetPos() { x_SetPos(0,0,0); }; - std::string GetTagName() const; - std::string GetChildTagName() const { return x_GetTagName(m_iPosChild); }; - std::string GetData() const { return x_GetData(m_iPos); }; - std::string GetChildData() const { return x_GetData(m_iPosChild); }; - std::string GetAttrib(const char* szAttrib) const { return x_GetAttrib(m_iPos,szAttrib); }; - std::string GetChildAttrib(const char* szAttrib) const { return x_GetAttrib(m_iPosChild,szAttrib); }; - std::string GetError() const { return m_csError; }; - - static std::string Format(const char *fmt, ...); - - enum MarkupNodeType - { - MNT_ELEMENT = 1, // 0x01 - MNT_TEXT = 2, // 0x02 - MNT_WHITESPACE = 4, // 0x04 - MNT_CDATA_SECTION = 8, // 0x08 - MNT_PROCESSING_INSTRUCTION = 16, // 0x10 - MNT_COMMENT = 32, // 0x20 - MNT_DOCUMENT_TYPE = 64, // 0x40 - MNT_EXCLUDE_WHITESPACE = 123,// 0x7b - }; - -protected: - -#ifdef _DEBUG - const char* m_pMainDS; - const char* m_pChildDS; -#endif - - std::string m_csDoc; - std::string m_csError; - - struct ElemPos - { - ElemPos() { Clear(); }; - ElemPos(const ElemPos& pos) { *this = pos; }; - bool IsEmptyElement() const { return (nStartR == nEndL + 1); }; - void Clear() - { - nStartL=0; nStartR=0; nEndL=0; nEndR=0; nReserved=0; - iElemParent=0; iElemChild=0; iElemNext=0; - }; - void AdjustStart(int n) { nStartL+=n; nStartR+=n; }; - void AdjustEnd(int n) { nEndL+=n; nEndR+=n; }; - int nStartL; - int nStartR; - int nEndL; - int nEndR; - int nReserved; - int iElemParent; - int iElemChild; - int iElemNext; - }; - - std::vector m_aPos; - int m_iPosParent; - int m_iPos; - int m_iPosChild; - int m_iPosFree; - int m_nNodeType; - - struct TokenPos - { - TokenPos(const char* sz) { Clear(); szDoc = sz; }; - bool IsValid() const { return (nL <= nR); }; - void Clear() { nL=0; nR=-1; nNext=0; bIsString=false; }; - bool Match(const char* szName) const; - int nL; - int nR; - int nNext; - const char* szDoc; - bool bIsString; - }; - - void x_SetPos(int iPosParent, int iPos, int iPosChild) - { - m_iPosParent = iPosParent; - m_iPos = iPos; - m_iPosChild = iPosChild; - m_nNodeType = iPos?MNT_ELEMENT:0; - MARKUP_SETDEBUGSTATE; - }; - - int x_GetFreePos(); - int x_ReleasePos(); - int x_ParseElem(int iPos); - int x_ParseError(const char* szError, const char* szName = NULL); - static bool x_FindChar(const char* szDoc, int& nChar, char c); - static bool x_FindAny(const char* szDoc, int& nChar); - static bool x_FindToken(TokenPos& token); - std::string x_GetToken(const TokenPos& token) const; - int x_FindElem(int iPosParent, int iPos, const char* szPath); - std::string x_GetTagName(int iPos) const; - std::string x_GetData(int iPos) const; - std::string x_GetAttrib(int iPos, const char* szAttrib) const; - bool x_AddElem(const char* szName, const char* szValue, bool bInsert, bool bAddChild); - bool x_FindAttrib(TokenPos& token, const char* szAttrib=NULL) const; - bool x_SetAttrib(int iPos, const char* szAttrib, const char* szValue); - void x_LocateNew(int iPosParent, int& iPosRel, int& nOffset, int nLength, int nFlags); - int x_ParseNode(TokenPos& token); - void x_DocChange(int nLeft, int nReplace, const std::string& csInsert); - void x_Adjust(int iPos, int nShift, bool bAfterPos = false); - std::string x_TextToDoc(const char* szText, bool bAttrib = false) const; - std::string x_TextFromDoc(int nLeft, int nRight) const; - -protected: - char mtIndent[ 1000 ]; - int mnIndent; - -private: - std::string Mid(const std::string &tStr, int nFirst) const; - std::string Mid(const std::string &tStr, int nFirst, int nCount) const; - char* GetBuffer(std::string &tStr, int nMinLen = -1) const; - void ReleaseBuffer(std::string &tStr, int nNewLen = -1) const; -}; - -#endif /* QUALISYS_NBC_NBC_MARKUP_H_INCLUDED */ diff --git a/navigation/qualisys_motion_capture/mocap_qualisys/include/mocap_qualisys/Network.cpp b/navigation/qualisys_motion_capture/mocap_qualisys/include/mocap_qualisys/Network.cpp deleted file mode 100644 index 992b8cc12..000000000 --- a/navigation/qualisys_motion_capture/mocap_qualisys/include/mocap_qualisys/Network.cpp +++ /dev/null @@ -1,578 +0,0 @@ -#define _CRT_SECURE_NO_WARNINGS -#define NOMINMAX - -#include "Network.h" - -#include -#include -#include -#include -#include -#include - -#ifdef _WIN32 -#include -#include -#else -#include // for inet_addr -#include // for sockaddr_in, ntohl, in_addr, etc -#include // for getsockname, send, AF_INET, etc -#include // for close, read, fork, etc -#include /* Solaris 2.5.1 fix: u_short, required by sys/socket.h */ -#include /* sockets */ -#include /* timeval */ -#include /* ioctl */ -#include /* bzero, for FD_SET */ -#include /* bzero, for FD_ZERO (AIX) */ -#include /* INADDR_*, in_addr, sockaddr_in, htonl etc. */ -#include // for TCP_NODELAY -#include /* getservbyname */ -#include /* inet_addr */ -#include /* socket error codes */ -#include - - -#define SOCKET_ERROR (-1) - -#define TIMEVAL timeval -#define closesocket close -#define ioctlsocket ioctl -#define SOCKADDR sockaddr -//#define SD_RECEIVE SHUT_RD -#define SD_SEND SHUT_WR -//#define SD_BOTH SHUT_RDWR - -#endif - -CNetwork::CNetwork() -{ - mSocket = INVALID_SOCKET; - mUDPSocket = INVALID_SOCKET; - mUDPBroadcastSocket = INVALID_SOCKET; - mLastError = 0; - mErrorStr[0] = 0; - - InitWinsock(); -} - - -CNetwork::~CNetwork() -{ -#ifdef _WIN32 - WSACleanup(); -#endif -} - - -bool CNetwork::InitWinsock() -{ -#ifdef _WIN32 - WORD wVersionRequested = MAKEWORD(2,2); - WSADATA wsaData; - - // Initialize WinSock and check version - if (WSAStartup(wVersionRequested, &wsaData) != 0) - { - SetErrorString(); - return false; - } - if (wsaData.wVersion != wVersionRequested) - { - SetErrorString(); - return false; - } -#endif - return true; -} // InitWinsock - - -bool CNetwork::Connect(const char* serverAddr, unsigned short nPort) -{ - mLastError = 0; - mErrorStr[0] = 0; - - // Connect to QTM RT server. - - mSocket = socket(AF_INET, SOCK_STREAM, 0); - if (mSocket == -1) - { - strcpy(mErrorStr, "Socket could not be created."); - } - - sockaddr_in sAddr; - - // First check if the address is a dotted number "A.B.C.D" - if (inet_pton(AF_INET, serverAddr, &(sAddr.sin_addr)) == 0) - { - // If it wasn't a dotted number lookup the server name - - struct addrinfo hints, *servinfo; - - memset(&hints, 0, sizeof hints); - hints.ai_family = AF_INET; - hints.ai_socktype = SOCK_STREAM; - - if (getaddrinfo(serverAddr, nullptr, &hints, &servinfo) != 0) - { - strcpy(mErrorStr, "Error looking up host name."); - closesocket(mSocket); - return false; - } - if (servinfo == nullptr) - { - strcpy(mErrorStr, "Error looking up host name."); - closesocket(mSocket); - return false; - } - sAddr.sin_addr = ((sockaddr_in *)servinfo[0].ai_addr)->sin_addr; - } - sAddr.sin_port = htons(nPort); - sAddr.sin_family = AF_INET; - - - if (connect(mSocket, (sockaddr*)(&sAddr), sizeof(sAddr)) == SOCKET_ERROR) - { - strcpy(mErrorStr, "Connect failed."); - - //SetErrorString(); - closesocket(mSocket); - return false; - } - - // Disable Nagle's algorithm -#ifdef _WIN32 - char bNoDelay = 1; -#else - int bNoDelay = 1; -#endif - if (setsockopt(mSocket, IPPROTO_TCP, TCP_NODELAY, &bNoDelay, sizeof(bNoDelay)) != 0) - { - SetErrorString(); - closesocket(mSocket); - return false; - } - - return true; -} // Connect - - -void CNetwork::Disconnect() -{ - // Try to shutdown gracefully - - shutdown(mSocket, SD_SEND); - closesocket(mSocket); - closesocket(mUDPSocket); - closesocket(mUDPBroadcastSocket); - mSocket = INVALID_SOCKET; - mUDPSocket = INVALID_SOCKET; - mUDPBroadcastSocket = INVALID_SOCKET; -} // Disconnect - - -bool CNetwork::Connected() const -{ - return mSocket != INVALID_SOCKET; -} - -bool CNetwork::CreateUDPSocket(unsigned short &nUDPPort, bool bBroadcast) -{ - if (nUDPPort == 0 || nUDPPort > 1023) - { - SOCKET tempSocket = INVALID_SOCKET; - - // Create UDP socket for data streaming - sockaddr_in recvAddr; - recvAddr.sin_family = AF_INET; - recvAddr.sin_port = htons(nUDPPort); - recvAddr.sin_addr.s_addr = INADDR_ANY; - - tempSocket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); - if (tempSocket != INVALID_SOCKET) - { - u_long argp = 1; - // Make socket unblocking. - if (ioctlsocket(tempSocket, FIONBIO, &argp) == 0) - { - if (bind(tempSocket, (SOCKADDR *) &recvAddr, sizeof(recvAddr)) != -1) - { - nUDPPort = GetUdpServerPort(tempSocket); - if (bBroadcast) - { -#ifdef _WIN32 - char broadcast = 1; -#else - int broadcast = 1; -#endif - if (setsockopt(tempSocket, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)) == 0) - { - mUDPBroadcastSocket = tempSocket; - return true; - } - else - { - strcpy(mErrorStr, "Failed to set socket options for UDP server socket."); - } - } - else - { - mUDPSocket = tempSocket; - return true; - } - } - else - { - strcpy(mErrorStr, "Failed to bind UDP server socket."); - } - } - else - { - strcpy(mErrorStr, "Failed to make UDP server socket unblocking."); - } - } - else - { - strcpy(mErrorStr, "Failed to create UDP server socket."); - } - closesocket(tempSocket); - } - - return false; -} - -unsigned short CNetwork::GetUdpServerPort(SOCKET socket) -{ - sockaddr_in recvAddr; - socklen_t addrLen = sizeof(recvAddr); - if (getsockname(socket, (struct sockaddr *)&recvAddr, &addrLen) == 0 && - recvAddr.sin_family == AF_INET && - addrLen == sizeof(recvAddr)) - { - return ntohs(recvAddr.sin_port); - } - return 0; -} - -unsigned short CNetwork::GetUdpServerPort() -{ - return GetUdpServerPort(mUDPSocket); -} - -unsigned short CNetwork::GetUdpBroadcastServerPort() -{ - return GetUdpServerPort(mUDPBroadcastSocket); -} - -// Receive a data packet. Data is stored in a local static buffer -// Returns number of bytes in received message, 0 on timeout or -1 if there is an error. -int CNetwork::Receive(char* rtDataBuff, int dataBufSize, bool header, int timeout, unsigned int *ipAddr) -{ - int recieved = 0; - sockaddr_in source_addr; - socklen_t fromlen = sizeof(source_addr); - - fd_set readFDs, exceptFDs; - FD_ZERO(&readFDs); - FD_ZERO(&exceptFDs); - - if (mSocket != INVALID_SOCKET) - { - FD_SET(mSocket, &readFDs); - FD_SET(mSocket, &exceptFDs); - } - if (mUDPSocket != INVALID_SOCKET) - { - FD_SET(mUDPSocket, &readFDs); - FD_SET(mUDPSocket, &exceptFDs); - } - if (mUDPBroadcastSocket != INVALID_SOCKET) - { - FD_SET(mUDPBroadcastSocket, &readFDs); - FD_SET(mUDPBroadcastSocket, &exceptFDs); - } - - TIMEVAL* pTimeval; - TIMEVAL sTimeval; - - if (timeout < 0) - { - pTimeval = nullptr; - } - else - { - sTimeval.tv_sec = timeout / 1000000; - sTimeval.tv_usec = timeout % 1000000; - pTimeval = &sTimeval; - } - -#ifdef _WIN32 - const int nfds = 0; -#else - const int nfds = std::max(mSocket, std::max(mUDPSocket, mUDPBroadcastSocket)) + 1; -#endif - - // Wait for activity on the TCP and UDP sockets. - int selectRes = select(nfds, &readFDs, nullptr, &exceptFDs, pTimeval); - if (selectRes == 0) - { - return 0; // Select timeout. - } - if (selectRes > 0) - { - if (FD_ISSET(mSocket, &exceptFDs)) - { - // General socket error - FD_CLR(mSocket, &exceptFDs); - SetErrorString(); - recieved = SOCKET_ERROR; - } - else if (FD_ISSET(mSocket, &readFDs)) - { - recieved = recv(mSocket, rtDataBuff, header ? 8 : dataBufSize, 0); - FD_CLR(mSocket, &readFDs); - } - else if (FD_ISSET(mUDPSocket, &exceptFDs)) - { - // General socket error - FD_CLR(mUDPSocket, &exceptFDs); - SetErrorString(); - recieved = SOCKET_ERROR; - } - else if (FD_ISSET(mUDPSocket, &readFDs)) - { - recieved = recvfrom(mUDPSocket, rtDataBuff, dataBufSize, 0, (sockaddr*)&source_addr, &fromlen); - FD_CLR(mUDPSocket, &readFDs); - } - else if (FD_ISSET(mUDPBroadcastSocket, &exceptFDs)) - { - // General socket error - FD_CLR(mUDPBroadcastSocket, &exceptFDs); - SetErrorString(); - recieved = SOCKET_ERROR; - } - else if (FD_ISSET(mUDPBroadcastSocket, &readFDs)) - { - recieved = recvfrom(mUDPBroadcastSocket, rtDataBuff, dataBufSize, 0, (sockaddr*)&source_addr, &fromlen); - FD_CLR(mUDPBroadcastSocket, &readFDs); - if (ipAddr) - { - *ipAddr = source_addr.sin_addr.s_addr; - } - } - } - else - { - recieved = -1; - } - - if (recieved == -1) - { - SetErrorString(); - } - return recieved; -} - - -bool CNetwork::Send(const char* sendBuf, int size) -{ - int sent = 0; - int totalSent = 0; - - while (totalSent < size) - { - sent = send(mSocket, sendBuf + totalSent, size - totalSent, 0); - if (sent == SOCKET_ERROR) - { - SetErrorString(); - return false; - } - totalSent += sent; - } - return true; -} - - -bool CNetwork::SendUDPBroadcast(const char* sendBuf, int size, short port, unsigned int filterAddr /* = 0 */) -{ - bool broadCastSent = false; - - if (mUDPBroadcastSocket != INVALID_SOCKET) - { -#ifdef _WIN32 - IP_ADAPTER_INFO* ifap = nullptr; - IP_ADAPTER_INFO* ifa = nullptr; - ULONG ulLen = 0; - DWORD erradapt; - - // Find all network interfaces. - erradapt = ::GetAdaptersInfo(ifap, &ulLen); - if (erradapt == ERROR_BUFFER_OVERFLOW) - { - ifap = (IP_ADAPTER_INFO*)malloc(ulLen); - erradapt = ::GetAdaptersInfo(ifap, &ulLen); - } - - if (erradapt == ERROR_SUCCESS) - { - sockaddr_in recvAddr; - recvAddr.sin_family = AF_INET; - recvAddr.sin_port = htons(port); - recvAddr.sin_addr.s_addr = 0xffffffff; - - // Send broadcast on all Ethernet interfaces. - ifa = ifap; - while (ifa) - { - if (ifa->Type == MIB_IF_TYPE_ETHERNET) - { - unsigned int nIPaddr; - unsigned int nIPmask; - - if (inet_pton(AF_INET, ifa->IpAddressList.IpAddress.String, &nIPaddr) == 0 || - inet_pton(AF_INET, ifa->IpAddressList.IpMask.String, &nIPmask) == 0) - { - return false; - } - recvAddr.sin_addr.s_addr = nIPaddr | (~nIPmask); - if (recvAddr.sin_addr.s_addr != (filterAddr | (~nIPmask))) - { - if (sendto(mUDPBroadcastSocket, sendBuf, size, 0, (sockaddr*)&recvAddr, sizeof(recvAddr)) == size) - { - broadCastSent = true; - } - } - } - ifa = ifa->Next; - } - } - free(ifap); -#else - // Find all network interfaces. - struct ifaddrs* ifap = nullptr; - if (getifaddrs(&ifap) == 0) - { - sockaddr_in recvAddr; - recvAddr.sin_family = AF_INET; - recvAddr.sin_port = htons(port); - recvAddr.sin_addr.s_addr = 0xffffffff; - - // Send broadcast on all Ethernet interfaces. - auto* ifa = ifap; - while (ifa) - { - if (ifa->ifa_addr->sa_family == AF_INET) - { - auto* sa = (struct sockaddr_in *) ifa->ifa_addr; - auto ipAddr = sa->sin_addr.s_addr; - - auto* saMask = (struct sockaddr_in *) ifa->ifa_netmask; - auto ipMask = saMask->sin_addr.s_addr; - - recvAddr.sin_addr.s_addr = ipAddr | (~ipMask); - if (recvAddr.sin_addr.s_addr != (filterAddr | (~ipMask))) - { - if (sendto(mUDPBroadcastSocket, sendBuf, size, 0, (sockaddr*)&recvAddr, sizeof(recvAddr)) == size) - { - broadCastSent = true; - } - } - } - ifa = ifa->ifa_next; - } - } - freeifaddrs(ifap); -#endif - } - - return broadCastSent; -} // SendUDPBroadcast - - -void CNetwork::SetErrorString() -{ -#ifdef _WIN32 - char* error = nullptr; - mLastError = GetLastError(); - FormatMessage(FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM, nullptr, mLastError, 0, reinterpret_cast(&error), 0, nullptr); - sprintf(mErrorStr, "%s", error); - LocalFree(error); -#else - mLastError = errno; - char* error = strerror(mLastError); - if (error != nullptr) - { - sprintf(mErrorStr, "%s", error); - } -#endif -} - - -char* CNetwork::GetErrorString() -{ - return mErrorStr; -} - - -int CNetwork::GetError() const -{ - return mLastError; -} - - -bool CNetwork::IsLocalAddress(unsigned int nAddr) const -{ -#ifdef _WIN32 - IP_ADAPTER_INFO* pAdptInfo = nullptr; - IP_ADAPTER_INFO* pNextAd = nullptr; - DWORD erradapt; - ULONG ulLen = 0; - - // Find all network interfaces. - erradapt = ::GetAdaptersInfo(pAdptInfo, &ulLen); - if (erradapt == ERROR_BUFFER_OVERFLOW) - { - pAdptInfo = (IP_ADAPTER_INFO*)malloc(ulLen); - erradapt = ::GetAdaptersInfo(pAdptInfo, &ulLen); - } - - if (erradapt == ERROR_SUCCESS) - { - pNextAd = pAdptInfo; - while(pNextAd) - { - if (pNextAd->Type == MIB_IF_TYPE_ETHERNET) - { - // Check if it's a response from a local interface. - unsigned int addr; - if (inet_pton(AF_INET, pNextAd->IpAddressList.IpAddress.String, &addr) != 0) - { - return addr == nAddr; - } - } - pNextAd = pNextAd->Next; - } - } - free(pAdptInfo); -#else - struct ifaddrs* pAdptInfo = nullptr; - struct ifaddrs* pNextAd = nullptr; - if (getifaddrs(&pAdptInfo) == 0) - { - pNextAd = pAdptInfo; - while (pNextAd) - { - if (pNextAd->ifa_addr->sa_family == AF_INET) - { - struct sockaddr_in* pNextAd_in = (struct sockaddr_in *)pNextAd->ifa_addr; - if (pNextAd_in->sin_addr.s_addr == nAddr) - { - return true; - } - } - pNextAd = pNextAd->ifa_next; - } - } - freeifaddrs(pAdptInfo); -#endif - return false; -} \ No newline at end of file diff --git a/navigation/qualisys_motion_capture/mocap_qualisys/include/mocap_qualisys/Network.h b/navigation/qualisys_motion_capture/mocap_qualisys/include/mocap_qualisys/Network.h deleted file mode 100644 index b2a98a0c3..000000000 --- a/navigation/qualisys_motion_capture/mocap_qualisys/include/mocap_qualisys/Network.h +++ /dev/null @@ -1,44 +0,0 @@ -#ifndef NETWORK_H -#define NETWORK_H - -#ifdef _WIN32 - #define WIN32_LEAN_AND_MEAN - #include -#else - #define INVALID_SOCKET -1 - #define SOCKET int -#endif - -class CNetwork -{ -public: - CNetwork(); - ~CNetwork(); - bool Connect(const char* pServerAddr, unsigned short nPort); - void Disconnect(); - bool Connected() const; - bool CreateUDPSocket(unsigned short &nUDPPort, bool bBroadcast = false); - int Receive(char* rtDataBuff, int nDataBufSize, bool bHeader, int nTimeout, unsigned int *ipAddr = nullptr); - bool Send(const char* pSendBuf, int nSize); - bool SendUDPBroadcast(const char* pSendBuf, int nSize, short nPort, unsigned int nFilterAddr = 0); - char* GetErrorString(); - int GetError() const; - bool IsLocalAddress(unsigned int nAddr) const; - unsigned short GetUdpServerPort(); - unsigned short GetUdpBroadcastServerPort(); - -private: - bool InitWinsock(); - void SetErrorString(); - unsigned short GetUdpServerPort(SOCKET nSocket); - -private: - SOCKET mSocket; - SOCKET mUDPSocket; - SOCKET mUDPBroadcastSocket; - char mErrorStr[256]; - unsigned long mLastError; -}; - - -#endif \ No newline at end of file diff --git a/navigation/qualisys_motion_capture/mocap_qualisys/include/mocap_qualisys/QualisysDriver.h b/navigation/qualisys_motion_capture/mocap_qualisys/include/mocap_qualisys/QualisysDriver.h deleted file mode 100644 index fd67e3ea9..000000000 --- a/navigation/qualisys_motion_capture/mocap_qualisys/include/mocap_qualisys/QualisysDriver.h +++ /dev/null @@ -1,135 +0,0 @@ -/* - * Copyright [2015] - * [Kartik Mohta ] - * [Ke Sun ] - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef QUALISYS_DRIVER_H -#define QUALISYS_DRIVER_H - -#include -#include -#include -#include -#include -#include -#include -#include - - - -namespace mocap{ - -class QualisysDriver: public MoCapDriverBase{ - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - /* - * @brief Constructor - * @param nh Ros node - */ - QualisysDriver(const ros::NodeHandle& n): - MoCapDriverBase (n), - max_accel (10.0), - frame_interval (0.01), - last_packet_time (0), - process_noise (Eigen::Matrix::Zero()), - measurement_noise (Eigen::Matrix::Zero()) - { - return; - } - - /* - * @brief Destructor - */ - ~QualisysDriver() { - disconnect(); - return; - } - - /* - * @brief init Initialize the object - * @return True if successfully initialized - */ - bool init(); - - /* - * @brief run Start acquiring data from the server - */ - bool run(); - - /* - * @brief disconnect Disconnect to the server - * @Note The function is called automatically when the - * destructor is called. - */ - void disconnect(); - - private: - // Disable the copy constructor and assign operator - QualisysDriver(const QualisysDriver& ); - QualisysDriver& operator=(const QualisysDriver& ); - - // Handle a frame which contains the info of all subjects - void handleFrame(); - - // Handle a the info of a single subject - void handleSubject(int sub_idx); - // void handleSubject(int sub_idx); - - // Unit converter - static double deg2rad; - - // Port of the server to be connected - int base_port; - - // Port that the server should stream UDP packets to. - // 0 indicates that TCP is used. - unsigned short udp_stream_port; - - // Minor version of the QTM protocol - int qtm_protocol_version; - - // Protocol to connect to the server - CRTProtocol port_protocol; - - // A pointer to the received packet - // (no need to initialize) - CRTPacket* prt_packet; - - // A set to hold the model names - std::set model_set; - - // Max acceleration - double max_accel; - - // Average time interval between two frames - double frame_interval; - - // time point for last frame - unsigned long last_packet_time; - - // Convariance matrices for initializing kalman filters - Eigen::Matrix process_noise; - Eigen::Matrix measurement_noise; - - // Timestamp stuff - double start_time_local_ = 0; - double start_time_packet_ = 0; -}; -} - - -#endif \ No newline at end of file diff --git a/navigation/qualisys_motion_capture/mocap_qualisys/include/mocap_qualisys/RTPacket.cpp b/navigation/qualisys_motion_capture/mocap_qualisys/include/mocap_qualisys/RTPacket.cpp deleted file mode 100644 index 5fc2a988c..000000000 --- a/navigation/qualisys_motion_capture/mocap_qualisys/include/mocap_qualisys/RTPacket.cpp +++ /dev/null @@ -1,1797 +0,0 @@ -#define _CRT_SECURE_NO_WARNINGS -#define NOMINMAX - -#include -#include -#include -#include - -#ifdef _WIN32 -#include -#else -#include -#endif - -#include "RTPacket.h" - -CRTPacket::CRTPacket(int nMajorVersion, int nMinorVersion, bool bBigEndian) -{ - mnMajorVersion = nMajorVersion; - mnMinorVersion = nMinorVersion; - mbBigEndian = bBigEndian; - - ClearData(); -} - -void CRTPacket::GetVersion(unsigned int &nMajorVersion, unsigned int &nMinorVersion) -{ - nMajorVersion = mnMajorVersion; - nMinorVersion = mnMinorVersion; -} - -void CRTPacket::SetVersion(unsigned int nMajorVersion, unsigned int nMinorVersion) -{ - mnMajorVersion = nMajorVersion; - mnMinorVersion = nMinorVersion; -} - -bool CRTPacket::GetEndianness() -{ - return mbBigEndian; -} - -void CRTPacket::SetEndianness(bool bBigEndian) -{ - mbBigEndian = bBigEndian; -} - -void CRTPacket::ClearData() -{ - mpData = nullptr; - mnComponentCount = 0; - mn2DCameraCount = 0; - mn2DLinCameraCount = 0; - mnImageCameraCount = 0; - mnAnalogDeviceCount = 0; - mnAnalogSingleDeviceCount = 0; - mnForcePlateCount = 0; - mnForceSinglePlateCount = 0; - mnGazeVectorCount = 0; - mnTimecodeCount = 0; - mSkeletonCount = 0; - memset(mpComponentData, 0, ComponentNone * 4); - memset(mp2DData, 0, MAX_CAMERA_COUNT * 4); - memset(mp2DLinData, 0, MAX_CAMERA_COUNT * 4); - memset(mpImageData, 0, MAX_CAMERA_COUNT * 4); - memset(mpAnalogData, 0, MAX_ANALOG_DEVICE_COUNT * 4); - memset(mpAnalogSingleData, 0, MAX_ANALOG_DEVICE_COUNT * 4); - memset(mpForceData, 0, MAX_FORCE_PLATE_COUNT * 4); - memset(mpForceSingleData, 0, MAX_FORCE_PLATE_COUNT * 4); - memset(mpGazeVectorData, 0, MAX_GAZE_VECTOR_COUNT * 4); - memset(mpSkeletonData, 0, MAX_SKELETON_COUNT * 4); -} - -void CRTPacket::SetData(char* ptr) -{ - unsigned int nComponent; - unsigned int nCamera, nDevice; - - mpData = ptr; - - mnComponentCount = 0; - mn2DCameraCount = 0; - mn2DLinCameraCount = 0; - mnImageCameraCount = 0; - mnAnalogDeviceCount = 0; - mnAnalogSingleDeviceCount = 0; - mnForcePlateCount = 0; - mnForceSinglePlateCount = 0; - mnGazeVectorCount = 0; - mnTimecodeCount = 0; - mSkeletonCount = 0; - - // Check if it's a data packet - if (GetType() == PacketData) - { - // Reset all component data pointers - for (nComponent = 1; nComponent < ComponentNone; nComponent++) - { - mpComponentData[nComponent - 1] = nullptr; - } - - char* pCurrentComponent = mpData + 24; - unsigned int nComponentType = SetByteOrder((unsigned int*)(pCurrentComponent + 4)); - - mnComponentCount = SetByteOrder((unsigned int*)(mpData + 20)); - - for (nComponent = 1; nComponent <= mnComponentCount && nComponentType > 0 && nComponentType < ComponentNone; nComponent++) - { - mpComponentData[nComponentType - 1] = pCurrentComponent; - - if (nComponentType == Component2d) - { - mn2DCameraCount = SetByteOrder((unsigned int*)(pCurrentComponent + 8)); - - mp2DData[0] = pCurrentComponent + 16; - for (nCamera = 1; nCamera < mn2DCameraCount; nCamera++) - { - if (mnMajorVersion > 1 || mnMinorVersion > 7) - { - mp2DData[nCamera] = mp2DData[nCamera - 1] + 5 + Get2DMarkerCount(nCamera - 1) * 12; - } - else - { - mp2DData[nCamera] = mp2DData[nCamera - 1] + 4 + Get2DMarkerCount(nCamera - 1) * 12; - } - } - } - if (nComponentType == Component2dLin) - { - mn2DLinCameraCount = SetByteOrder((unsigned int*)(pCurrentComponent + 8)); - - mp2DLinData[0] = pCurrentComponent + 16; - for (nCamera = 1; nCamera < mn2DLinCameraCount; nCamera++) - { - if (mnMajorVersion > 1 || mnMinorVersion > 7) - { - mp2DLinData[nCamera] = mp2DLinData[nCamera - 1] + 5 + Get2DLinMarkerCount(nCamera - 1) * 12; - } - else - { - mp2DLinData[nCamera] = mp2DLinData[nCamera - 1] + 4 + Get2DLinMarkerCount(nCamera - 1) * 12; - } - } - } - if (nComponentType == ComponentImage) - { - mnImageCameraCount = SetByteOrder((unsigned int*)(pCurrentComponent + 8)); - - mpImageData[0] = pCurrentComponent + 12; - for (nCamera = 1; nCamera < mnImageCameraCount; nCamera++) - { - mpImageData[nCamera] = mpImageData[nCamera - 1] + 36 + SetByteOrder((unsigned int*)(mpImageData[nCamera - 1] + 32)); - } - } - if (nComponentType == ComponentAnalog) - { - if ((mnMajorVersion == 1) && (mnMinorVersion == 0)) - { - mnAnalogDeviceCount = 1; - } - else - { - mnAnalogDeviceCount = SetByteOrder((unsigned int*)(pCurrentComponent + 8)); - } - - if ((mnMajorVersion > 1) || (mnMinorVersion > 7)) - { - mpAnalogData[0] = pCurrentComponent + 12; - } - else - { - mpAnalogData[0] = pCurrentComponent + 16; - } - for (nDevice = 1; nDevice < mnAnalogDeviceCount; nDevice++) - { - mpAnalogData[nDevice] = mpAnalogData[nDevice - 1] + 16 + - (SetByteOrder((unsigned int*)(mpAnalogData[nDevice - 1] + 4)) * - SetByteOrder((unsigned int*)(mpAnalogData[nDevice - 1] + 8)) * 4); - } - } - if (nComponentType == ComponentAnalogSingle) - { - mnAnalogSingleDeviceCount = SetByteOrder((unsigned int*)(pCurrentComponent + 8)); - - if (mnMajorVersion > 1 || mnMinorVersion > 7) - { - mpAnalogSingleData[0] = pCurrentComponent + 12; - } - else - { - mpAnalogSingleData[0] = pCurrentComponent + 16; - } - - for (nDevice = 1; nDevice < mnAnalogSingleDeviceCount; nDevice++) - { - mpAnalogSingleData[nDevice] = mpAnalogSingleData[nDevice - 1] + 8 + - SetByteOrder((unsigned int*)(mpAnalogSingleData[nDevice - 1] + 4)) * 4; - } - } - if (nComponentType == ComponentForce) - { - mnForcePlateCount = SetByteOrder((unsigned int*)(pCurrentComponent + 8)); - - if (mnMajorVersion > 1 || mnMinorVersion > 7) - { - mpForceData[0] = pCurrentComponent + 12; - } - else - { - mpForceData[0] = pCurrentComponent + 16; - } - for (nDevice = 1; nDevice < mnForcePlateCount; nDevice++) - { - if ((mnMajorVersion == 1) && (mnMinorVersion == 0)) - { - mpForceData[nDevice] = mpForceData[nDevice - 1] + 72; - } - else - { - mpForceData[nDevice] = mpForceData[nDevice - 1] + 12 + - SetByteOrder((unsigned int*)(mpForceData[nDevice - 1] + 4)) * 36; - } - } - } - if (nComponentType == ComponentForceSingle) - { - mnForceSinglePlateCount = SetByteOrder((unsigned int*)(pCurrentComponent + 8)); - - mpForceSingleData[0] = pCurrentComponent + 12; - - for (nDevice = 1; nDevice < mnForceSinglePlateCount; nDevice++) - { - mpForceSingleData[nDevice] = mpForceSingleData[nDevice - 1] + 4 + 36; - } - } - if (nComponentType == ComponentGazeVector) - { - mnGazeVectorCount = SetByteOrder((unsigned int*)(pCurrentComponent + 8)); - - mpGazeVectorData[0] = pCurrentComponent + 12; - - for (nDevice = 1; nDevice < mnGazeVectorCount; nDevice++) - { - unsigned int nPrevSampleCount = SetByteOrder((unsigned int*)(mpGazeVectorData[nDevice - 1])); - mpGazeVectorData[nDevice] = mpGazeVectorData[nDevice - 1] + 4 + ((nPrevSampleCount == 0) ? 0 : 4) + - nPrevSampleCount * 24; - } - } - if (nComponentType == ComponentTimecode) - { - mnTimecodeCount = SetByteOrder((unsigned int*)(pCurrentComponent + 8)); - - mpTimecodeData[0] = pCurrentComponent + 12; - - for (nDevice = 1; nDevice < mnTimecodeCount; nDevice++) - { - mpTimecodeData[nDevice] = mpTimecodeData[nDevice - 1] + 12; - } - } - if (nComponentType == ComponentSkeleton) - { - mSkeletonCount = SetByteOrder((unsigned int*)(pCurrentComponent + 8)); - - mpSkeletonData[0] = pCurrentComponent + 12; - - for (nDevice = 1; nDevice < mSkeletonCount; nDevice++) - { - unsigned int prevSegmentCount = SetByteOrder((unsigned int*)(mpSkeletonData[nDevice - 1])); - mpSkeletonData[nDevice] = mpSkeletonData[nDevice - 1] + 4 + prevSegmentCount * 32; - } - } - pCurrentComponent += SetByteOrder((int*)pCurrentComponent); - nComponentType = SetByteOrder((unsigned int*)(pCurrentComponent + 4)); - } - } -} // SetData - - -void CRTPacket::GetData(char* &ptr, unsigned int &nSize) -{ - if (mpData == nullptr) - { - nSize = 0; - } - else - { - ptr = mpData; - nSize = *((int*)mpData); - } -} - -unsigned int CRTPacket::GetSize() -{ - if (mpData == nullptr) - { - return 0; - } - if (mbBigEndian || ((mnMajorVersion == 1) && (mnMinorVersion == 0))) - { - return ntohl(*((unsigned int*)mpData)); - } - return *((unsigned int*)mpData); -} - -CRTPacket::EPacketType CRTPacket::GetType() -{ - if (GetSize() < 8) - { - return PacketNone; - } - if (mbBigEndian || ((mnMajorVersion == 1) && (mnMinorVersion == 0))) - { - return (EPacketType)ntohl(*(unsigned int*)(mpData + 4)); - } - return (EPacketType)*((unsigned int*)(mpData + 4)); -} - -unsigned long long CRTPacket::GetTimeStamp() -{ - if (GetType() == PacketData) - { - return SetByteOrder((long long*)(mpData + 8)); - } - return 0; -} - -unsigned int CRTPacket::GetFrameNumber() -{ - if (GetType() == PacketData) - { - return SetByteOrder((unsigned int*)(mpData + 16)); - } - return 0; -} - -unsigned int CRTPacket::GetSize(char* pData, bool bBigEndian) -{ - if (bBigEndian) - { - return ntohl(*((unsigned int*)pData)); - } - return *((unsigned int*)pData); -} - -CRTPacket::EPacketType CRTPacket::GetType(char* pData, bool bBigEndian) -{ - if (GetSize(pData, bBigEndian) < 8) - { - return PacketNone; - } - if (bBigEndian) - { - return (EPacketType)ntohl(*(unsigned int*)(pData + 4)); - } - return (EPacketType)*((unsigned int*)(pData + 4)); -} - -unsigned long long CRTPacket::GetTimeStamp(char* pData, bool bBigEndian) -{ - if (GetType(pData, bBigEndian) == PacketData) - { - if (bBigEndian) - { - return ((unsigned long long)(ntohl((long)*((long long*)(pData + 8)))) << 32) + ntohl(*((long long*)(pData + 8)) >> 32); - } - return *((long long*)(pData + 8)); - } - return 0; -} - -unsigned int CRTPacket::GetFrameNumber(char* pData, bool bBigEndian) -{ - if (GetType(pData, bBigEndian) == PacketData) - { - if (bBigEndian) - { - return ntohl(*((unsigned int*)(pData + 16))); - } - return *((unsigned int*)(pData + 16)); - } - return 0; -} - -unsigned int CRTPacket::GetComponentCount() -{ - return mnComponentCount; -} - -unsigned int CRTPacket::GetComponentSize(EComponentType eComponent) -{ - if (eComponent < Component3d || eComponent >= ComponentNone) - { - return 0; - } - if (mpComponentData[eComponent - 1] == nullptr) - { - return 0; - } - return SetByteOrder((unsigned int*)(mpComponentData[eComponent - 1])); -} - -char* CRTPacket::GetErrorString() -{ - if (GetType() == PacketError) - { - return mpData + 8; - } - return nullptr; -} - - -char* CRTPacket::GetCommandString() -{ - if (GetType() == PacketCommand) - { - return mpData + 8; - } - return nullptr; -} - - -char* CRTPacket::GetCommandString(char* pData, bool bBigEndian) -{ - if (GetType(pData, bBigEndian) == PacketCommand) - { - return pData + 8; - } - return nullptr; -} - - -char* CRTPacket::GetXMLString() -{ - if (GetType() == PacketXML) - { - return mpData + 8; - } - return nullptr; -} - - -short CRTPacket::GetDiscoverResponseBasePort() -{ - if (GetType() == PacketCommand) - { - if (GetSize() == (8 + strlen(mpData + 8) + 1 + 2)) - { - return ntohs(*((short*)(mpData + 8 + strlen(mpData + 8) + 1))); - } - } - return 0; -} - -short CRTPacket::GetDiscoverResponseBasePort(char* pData, bool bBigEndian) -{ - if (GetType(pData, bBigEndian) == PacketCommand) - { - if (GetSize(pData, bBigEndian) == (8 + strlen(pData + 8) + 1 + 2)) - { - return ntohs(*((short*)(pData + 8 + strlen(pData + 8) + 1))); - } - } - return 0; -} - -bool CRTPacket::GetEvent(EEvent &eEvent) -{ - if (GetType() == PacketEvent) - { - eEvent = (EEvent)*(mpData + 8); - return true; - } - return false; -} - -bool CRTPacket::GetEvent(EEvent &eEvent, char* pData, bool bBigEndian) -{ - if (GetType(pData, bBigEndian) == PacketEvent) - { - eEvent = (EEvent)*(pData + 8); - return true; - } - return false; -} - -unsigned short CRTPacket::GetDropRate() -{ - for (int i = 0; i <= 1; i++) - { - if (mpComponentData[i] != nullptr) - { - return SetByteOrder((unsigned short*)(mpComponentData[i] + 12)); - } - } - for (int i = 4; i <= 11; i++) - { - if (mpComponentData[i] != nullptr) - { - return SetByteOrder((unsigned short*)(mpComponentData[i] + 12)); - } - } - return 0; -} - -unsigned short CRTPacket::GetOutOfSyncRate() -{ - for (int i = 0; i <= 1; i++) - { - if (mpComponentData[i] != nullptr) - { - return SetByteOrder((unsigned short*)(mpComponentData[i] + 14)); - } - } - for (int i = 4; i <= 11; i++) - { - if (mpComponentData[i] != nullptr) - { - return SetByteOrder((unsigned short*)(mpComponentData[i] + 14)); - } - } - return 0; -} - - - -//----------------------------------------------------------- -// 2D -//----------------------------------------------------------- -unsigned int CRTPacket::Get2DCameraCount() -{ - return mn2DCameraCount; -} - -unsigned int CRTPacket::Get2DMarkerCount(unsigned int nCameraIndex) -{ - if (mn2DCameraCount <= nCameraIndex) - { - return 0; - } - return SetByteOrder((unsigned int*)(mp2DData[nCameraIndex])); -} - -unsigned char CRTPacket::Get2DStatusFlags(unsigned int nCameraIndex) -{ - if (mn2DCameraCount > nCameraIndex && ((mnMajorVersion > 1) || (mnMinorVersion > 7))) - { - return *((unsigned char*)(mp2DData[nCameraIndex] + 4)); - } - return 0; -} - -bool CRTPacket::Get2DMarker(unsigned int nCameraIndex, unsigned int nMarkerIndex, unsigned int &nX, unsigned int &nY, - unsigned short &nXDiameter, unsigned short &nYDiameter) -{ - int nOffset; - - if (mn2DCameraCount <= nCameraIndex || Get2DMarkerCount(nCameraIndex) <= nMarkerIndex) - { - return false; - } - - if (mnMajorVersion > 1 || mnMinorVersion > 7) - { - nOffset = 5; - } - else - { - nOffset = 4; - } - nX = SetByteOrder((unsigned int*)(mp2DData[nCameraIndex] + nOffset + nMarkerIndex * 12)); - nY = SetByteOrder((unsigned int*)(mp2DData[nCameraIndex] + nOffset + 4 + nMarkerIndex * 12)); - nXDiameter = SetByteOrder((unsigned short*)(mp2DData[nCameraIndex] + nOffset + 8 + nMarkerIndex * 12)); - nYDiameter = SetByteOrder((unsigned short*)(mp2DData[nCameraIndex] + nOffset + 10 + nMarkerIndex * 12)); - - return true; -} - - -//----------------------------------------------------------- -// 2D Linearized -//----------------------------------------------------------- -unsigned int CRTPacket::Get2DLinCameraCount() -{ - return mn2DLinCameraCount; -} - -unsigned int CRTPacket::Get2DLinMarkerCount(unsigned int nCameraIndex) -{ - if (mn2DLinCameraCount <= nCameraIndex) - { - return 0; - } - return SetByteOrder((unsigned int*)(mp2DLinData[nCameraIndex])); -} - -unsigned char CRTPacket::Get2DLinStatusFlags(unsigned int nCameraIndex) -{ - if (mn2DLinCameraCount > nCameraIndex && ((mnMajorVersion > 1) || (mnMinorVersion > 7))) - { - return *((unsigned char*)(mp2DLinData[nCameraIndex] + 4)); - } - return 0; -} - -bool CRTPacket::Get2DLinMarker(unsigned int nCameraIndex, unsigned int nMarkerIndex, unsigned int &nX, unsigned int &nY, - unsigned short &nXDiameter, unsigned short &nYDiameter) -{ - int nOffset; - - if (mn2DLinCameraCount <= nCameraIndex || Get2DLinMarkerCount(nCameraIndex) <= nMarkerIndex) - { - return false; - } - - if (mnMajorVersion > 1 || mnMinorVersion > 7) - { - nOffset = 5; - } - else - { - nOffset = 4; - } - nX = SetByteOrder((unsigned int*)(mp2DLinData[nCameraIndex] + nOffset + nMarkerIndex * 12)); - nY = SetByteOrder((unsigned int*)(mp2DLinData[nCameraIndex] + nOffset + 4 + nMarkerIndex * 12)); - nXDiameter = SetByteOrder((unsigned short*)(mp2DLinData[nCameraIndex] + nOffset + 8 + nMarkerIndex * 12)); - nYDiameter = SetByteOrder((unsigned short*)(mp2DLinData[nCameraIndex] + nOffset + 10 + nMarkerIndex * 12)); - - return true; -} - - -//----------------------------------------------------------- -// 3D -//----------------------------------------------------------- -unsigned int CRTPacket::Get3DMarkerCount() -{ - if (GetComponentSize(CRTPacket::Component3d) == 0) - { - return 0; - } - - char* pData = mpComponentData[Component3d - 1]; - - if (pData == nullptr) - { - return 0; - } - return SetByteOrder((unsigned int*)(pData + 8)); -} - -bool CRTPacket::Get3DMarker(unsigned int nMarkerIndex, float &fX, float &fY, float &fZ) -{ - char* pData = mpComponentData[Component3d - 1]; - - if (Get3DMarkerCount() <= nMarkerIndex) - { - return false; - } - - if (mnMajorVersion > 1 || mnMinorVersion > 7) - { - fX = SetByteOrder((float*)(pData + 16 + nMarkerIndex * 12)); - fY = SetByteOrder((float*)(pData + 20 + nMarkerIndex * 12)); - fZ = SetByteOrder((float*)(pData + 24 + nMarkerIndex * 12)); - } - else - { - fX = (float)SetByteOrder((double*)(pData + 16 + nMarkerIndex * 24)); - fY = (float)SetByteOrder((double*)(pData + 24 + nMarkerIndex * 24)); - fZ = (float)SetByteOrder((double*)(pData + 32 + nMarkerIndex * 24)); - } - return (isnan(fX) == 0); -} - - -//----------------------------------------------------------- -// 3D Residual -//----------------------------------------------------------- -unsigned int CRTPacket::Get3DResidualMarkerCount() -{ - char* pData = mpComponentData[Component3dRes - 1]; - - if (pData == nullptr) - { - return 0; - } - return SetByteOrder((unsigned int*)(pData + 8)); -} - -bool CRTPacket::Get3DResidualMarker(unsigned int nMarkerIndex, float &fX, float &fY, float &fZ, float &fResidual) -{ - char* pData = mpComponentData[Component3dRes - 1]; - - if (Get3DResidualMarkerCount() <= nMarkerIndex) - { - return false; - } - - if (mnMajorVersion > 1 || mnMinorVersion > 7) - { - fX = SetByteOrder((float*)(pData + 16 + nMarkerIndex * 16)); - fY = SetByteOrder((float*)(pData + 20 + nMarkerIndex * 16)); - fZ = SetByteOrder((float*)(pData + 24 + nMarkerIndex * 16)); - fResidual = SetByteOrder((float*)(pData + 28 + nMarkerIndex * 16)); - } - else - { - fX = (float)SetByteOrder((double*)(pData + 16 + nMarkerIndex * 32)); - fY = (float)SetByteOrder((double*)(pData + 24 + nMarkerIndex * 32)); - fZ = (float)SetByteOrder((double*)(pData + 32 + nMarkerIndex * 32)); - fResidual = SetByteOrder((float*) (pData + 40 + nMarkerIndex * 32)); - } - return (isnan(fX) == 0); -} - - -//----------------------------------------------------------- -// 3D No Labels -//----------------------------------------------------------- -unsigned int CRTPacket::Get3DNoLabelsMarkerCount() -{ - char* pData = mpComponentData[Component3dNoLabels - 1]; - - if (pData == nullptr) - { - return 0; - } - return SetByteOrder((unsigned int*)(pData + 8)); -} - -bool CRTPacket::Get3DNoLabelsMarker(unsigned int nMarkerIndex, float &fX, float &fY, float &fZ, unsigned int &nId) -{ - char* pData = mpComponentData[Component3dNoLabels - 1]; - - if (Get3DNoLabelsMarkerCount() <= nMarkerIndex) - { - return false; - } - - if (mnMajorVersion > 1 || mnMinorVersion > 7) - { - fX = SetByteOrder((float*)(pData + 16 + nMarkerIndex * 16)); - fY = SetByteOrder((float*)(pData + 20 + nMarkerIndex * 16)); - fZ = SetByteOrder((float*)(pData + 24 + nMarkerIndex * 16)); - nId = SetByteOrder((unsigned int*)(pData + 28 + nMarkerIndex * 16)); - } - else - { - fX = (float)SetByteOrder((double*)(pData + 16 + nMarkerIndex * 32)); - fY = (float)SetByteOrder((double*)(pData + 24 + nMarkerIndex * 32)); - fZ = (float)SetByteOrder((double*)(pData + 32 + nMarkerIndex * 32)); - nId = SetByteOrder((unsigned int*)(pData + 40 + nMarkerIndex * 32)); - } - return true; -} - - -//----------------------------------------------------------- -// 3D No Labels Residual -//----------------------------------------------------------- -unsigned int CRTPacket::Get3DNoLabelsResidualMarkerCount() -{ - char* pData = mpComponentData[Component3dNoLabelsRes - 1]; - - if (pData == nullptr) - { - return 0; - } - return SetByteOrder((unsigned int*)(pData + 8)); -} - -bool CRTPacket::Get3DNoLabelsResidualMarker(unsigned int nMarkerIndex, float &fX, float &fY, float &fZ, - unsigned int &nId, float &fResidual) -{ - char* pData = mpComponentData[Component3dNoLabelsRes - 1]; - - if (Get3DNoLabelsResidualMarkerCount() <= nMarkerIndex) - { - return false; - } - - if (mnMajorVersion > 1 || mnMinorVersion > 7) - { - fX = SetByteOrder((float*)(pData + 16 + nMarkerIndex * 20)); - fY = SetByteOrder((float*)(pData + 20 + nMarkerIndex * 20)); - fZ = SetByteOrder((float*)(pData + 24 + nMarkerIndex * 20)); - nId = SetByteOrder((unsigned int*)(pData + 28 + nMarkerIndex * 20)); - fResidual = SetByteOrder((float*)(pData + 32 + nMarkerIndex * 20)); - } - else - { - fX = (float)SetByteOrder((double*)(pData + 16 + nMarkerIndex * 32)); - fY = (float)SetByteOrder((double*)(pData + 24 + nMarkerIndex * 32)); - fZ = (float)SetByteOrder((double*)(pData + 32 + nMarkerIndex * 32)); - nId = SetByteOrder((unsigned int*)(pData + 40 + nMarkerIndex * 32)); - fResidual = SetByteOrder((float*) (pData + 44 + nMarkerIndex * 32)); - } - return true; -} - - -//----------------------------------------------------------- -// 6DOF -//----------------------------------------------------------- -unsigned int CRTPacket::Get6DOFBodyCount() -{ - char* pData = mpComponentData[Component6d - 1]; - - if (pData == nullptr) - { - return 0; - } - return SetByteOrder((unsigned int*)(pData + 8)); -} - -bool CRTPacket::Get6DOFBody(unsigned int nBodyIndex, float &fX, float &fY, float &fZ, float afRotMatrix[9]) -{ - char* pData = mpComponentData[Component6d - 1]; - - if (Get6DOFBodyCount() <= nBodyIndex) - { - return false; - } - - if (mnMajorVersion > 1 || mnMinorVersion > 7) - { - fX = SetByteOrder((float*)(pData + 16 + nBodyIndex * 48)); - fY = SetByteOrder((float*)(pData + 20 + nBodyIndex * 48)); - fZ = SetByteOrder((float*)(pData + 24 + nBodyIndex * 48)); - for (int i = 0; i < 9; i++) - { - afRotMatrix[i] = SetByteOrder((float*)(pData + 28 + (i * 4) + nBodyIndex * 48)); - } - } - else - { - fX = (float)SetByteOrder((double*)(pData + 16 + nBodyIndex * 96)); - fY = (float)SetByteOrder((double*)(pData + 24 + nBodyIndex * 96)); - fZ = (float)SetByteOrder((double*)(pData + 32 + nBodyIndex * 96)); - for (int i = 0; i < 9; i++) - { - afRotMatrix[i] = (float)SetByteOrder((float*)(pData + 40 + (i * 4) + nBodyIndex * 96)); - } - } - return true; -} - - -//----------------------------------------------------------- -// 6DOF Residual -//----------------------------------------------------------- -unsigned int CRTPacket::Get6DOFResidualBodyCount() -{ - char* pData = mpComponentData[Component6dRes - 1]; - - if (pData == nullptr) - { - return 0; - } - return SetByteOrder((unsigned int*)(pData + 8)); -} - -bool CRTPacket::Get6DOFResidualBody(unsigned int nBodyIndex, float &fX, float &fY, float &fZ, - float afRotMatrix[9], float &fResidual) -{ - char* pData = mpComponentData[Component6dRes - 1]; - - if (Get6DOFResidualBodyCount() <= nBodyIndex) - { - return false; - } - - if (mnMajorVersion > 1 || mnMinorVersion > 7) - { - fX = SetByteOrder((float*)(pData + 16 + nBodyIndex * 52)); - fY = SetByteOrder((float*)(pData + 20 + nBodyIndex * 52)); - fZ = SetByteOrder((float*)(pData + 24 + nBodyIndex * 52)); - for (int i = 0; i < 9; i++) - { - afRotMatrix[i] = SetByteOrder((float*)(pData + 28 + (i * 4) + nBodyIndex * 52)); - } - fResidual = SetByteOrder((float*)(pData + 64 + nBodyIndex * 52)); - } - else - { - fX = (float)SetByteOrder((double*)(pData + 16 + nBodyIndex * 104)); - fY = (float)SetByteOrder((double*)(pData + 24 + nBodyIndex * 104)); - fZ = (float)SetByteOrder((double*)(pData + 32 + nBodyIndex * 104)); - for (int i = 0; i < 9; i++) - { - afRotMatrix[i] = (float)SetByteOrder((double*)(pData + 40 + (i * 8) + nBodyIndex * 104)); - } - fResidual = SetByteOrder((float*)(pData + 112 + nBodyIndex * 104)); - } - return true; -} - - -//----------------------------------------------------------- -// 6DOF Euler -//----------------------------------------------------------- -unsigned int CRTPacket::Get6DOFEulerBodyCount() -{ - char* pData = mpComponentData[Component6dEuler - 1]; - - if (pData == nullptr) - { - return 0; - } - return SetByteOrder((unsigned int*)(pData + 8)); -} - -bool CRTPacket::Get6DOFEulerBody(unsigned int nBodyIndex, float &fX, float &fY, float &fZ, - float &fAng1, float &fAng2, float &fAng3) -{ - char* pData = mpComponentData[Component6dEuler - 1]; - - if (Get6DOFEulerBodyCount() <= nBodyIndex) - { - return false; - } - - if (mnMajorVersion > 1 || mnMinorVersion > 7) - { - fX = SetByteOrder((float*)(pData + 16 + nBodyIndex * 24)); - fY = SetByteOrder((float*)(pData + 20 + nBodyIndex * 24)); - fZ = SetByteOrder((float*)(pData + 24 + nBodyIndex * 24)); - fAng1 = SetByteOrder((float*)(pData + 28 + nBodyIndex * 24)); - fAng2 = SetByteOrder((float*)(pData + 32 + nBodyIndex * 24)); - fAng3 = SetByteOrder((float*)(pData + 36 + nBodyIndex * 24)); - } - else - { - fX = (float)SetByteOrder((double*)(pData + 16 + nBodyIndex * 48)); - fY = (float)SetByteOrder((double*)(pData + 24 + nBodyIndex * 48)); - fZ = (float)SetByteOrder((double*)(pData + 32 + nBodyIndex * 48)); - fAng1 = (float)SetByteOrder((double*)(pData + 40 + nBodyIndex * 48)); - fAng2 = (float)SetByteOrder((double*)(pData + 48 + nBodyIndex * 48)); - fAng3 = (float)SetByteOrder((double*)(pData + 56 + nBodyIndex * 48)); - } - return true; -} - - -//----------------------------------------------------------- -// 6DOF Euler Residual -//----------------------------------------------------------- -unsigned int CRTPacket::Get6DOFEulerResidualBodyCount() -{ - char* pData = mpComponentData[Component6dEulerRes - 1]; - - if (pData == nullptr) - { - return 0; - } - return SetByteOrder((unsigned int*)(pData + 8)); -} - -bool CRTPacket::Get6DOFEulerResidualBody(unsigned int nBodyIndex, float &fX, float &fY, float &fZ, - float &fAng1, float &fAng2, float &fAng3, float &fResidual) -{ - char* pData = mpComponentData[Component6dEulerRes - 1]; - - if (Get6DOFEulerResidualBodyCount() <= nBodyIndex) - { - return false; - } - - if (mnMajorVersion > 1 || mnMinorVersion > 7) - { - fX = SetByteOrder((float*)(pData + 16 + nBodyIndex * 28)); - fY = SetByteOrder((float*)(pData + 20 + nBodyIndex * 28)); - fZ = SetByteOrder((float*)(pData + 24 + nBodyIndex * 28)); - fAng1 = SetByteOrder((float*)(pData + 28 + nBodyIndex * 28)); - fAng2 = SetByteOrder((float*)(pData + 32 + nBodyIndex * 28)); - fAng3 = SetByteOrder((float*)(pData + 36 + nBodyIndex * 28)); - fResidual = SetByteOrder((float*)(pData + 40 + nBodyIndex * 28)); - } - else - { - fX = (float)SetByteOrder((double*)(pData + 16 + nBodyIndex * 56)); - fY = (float)SetByteOrder((double*)(pData + 24 + nBodyIndex * 56)); - fZ = (float)SetByteOrder((double*)(pData + 32 + nBodyIndex * 56)); - fAng1 = (float)SetByteOrder((double*)(pData + 40 + nBodyIndex * 56)); - fAng2 = (float)SetByteOrder((double*)(pData + 48 + nBodyIndex * 56)); - fAng3 = (float)SetByteOrder((double*)(pData + 56 + nBodyIndex * 56)); - fResidual = SetByteOrder( (float*)(pData + 64 + nBodyIndex * 56)); - } - return true; -} - - -//----------------------------------------------------------- -// Gaze Vector -//----------------------------------------------------------- -unsigned int CRTPacket::GetGazeVectorCount() -{ - return mnGazeVectorCount; -} - -unsigned int CRTPacket::GetGazeVectorSampleCount(unsigned int nVectorIndex) -{ - if (mnGazeVectorCount <= nVectorIndex) - { - return 0; - } - return SetByteOrder((unsigned int*)(mpGazeVectorData[nVectorIndex])); -} - -unsigned int CRTPacket::GetGazeVectorSampleNumber(unsigned int nVectorIndex) -{ - unsigned int nSampleCount = GetGazeVectorSampleCount(nVectorIndex); - - if (nSampleCount == 0) - { - return 0; - } - return SetByteOrder((unsigned int*)(mpGazeVectorData[nVectorIndex] + 4)); -} - -bool CRTPacket::GetGazeVector(unsigned int nVectorIndex, unsigned int nSampleIndex, SGazeVector &sGazeVector) -{ - unsigned int nSampleCount = GetGazeVectorSampleCount(nVectorIndex); - - if (nSampleCount == 0 || nSampleIndex >= nSampleCount) - { - return false; - } - - for (unsigned int k = 0; k < 6; k++) - { - *(((float*)&sGazeVector) + k) = - (float)SetByteOrder((float*)(mpGazeVectorData[nVectorIndex] + 8 + k * sizeof(float) + nSampleIndex * 24)); - } - - return (isnan(sGazeVector.fPosX) == 0); -} - -bool CRTPacket::GetGazeVector(unsigned int nVectorIndex, SGazeVector* pGazeVectorBuf, unsigned int nBufSize) -{ - unsigned int nSampleCount = GetGazeVectorSampleCount(nVectorIndex); - - if (nSampleCount == 0 || (nBufSize < nSampleCount * sizeof(SGazeVector))) - { - return false; - } - - for (unsigned int nSample = 0; nSample < nSampleCount; nSample++) - { - for (unsigned int k = 0; k < 6; k++) - { - *(((float*)pGazeVectorBuf) + k + (nSample * sizeof(SGazeVector))) = - (float)SetByteOrder((float*)(mpGazeVectorData[nVectorIndex] + 8 + k * sizeof(float) + nSample * 24)); - } - } - - return true; -} - - -//----------------------------------------------------------- -// Timecode -//----------------------------------------------------------- -unsigned int CRTPacket::GetTimecodeCount() -{ - return mnTimecodeCount; -} - -bool CRTPacket::GetTimecodeType(unsigned int nTimecodeIndex, CRTPacket::ETimecodeType &timecodeType) -{ - if (mnTimecodeCount <= nTimecodeIndex) - { - return false; - } - timecodeType = (CRTPacket::ETimecodeType)SetByteOrder((unsigned int*)(mpTimecodeData[nTimecodeIndex])); - return true; -} - -bool CRTPacket::GetTimecodeSMPTE(unsigned int nTimecodeIndex, int &hours, int &minutes, int &seconds, int &frame) -{ - if (mnTimecodeCount <= nTimecodeIndex) - { - return false; - } - CRTPacket::ETimecodeType timecodeType; - if (GetTimecodeType(nTimecodeIndex, timecodeType)) - { - if (timecodeType == TimecodeSMPTE) - { - hours = 0x1f & SetByteOrder((unsigned int*)(mpTimecodeData[nTimecodeIndex] + 8)); - minutes = 0x3f & (SetByteOrder((unsigned int*)(mpTimecodeData[nTimecodeIndex] + 8)) >> 5); - seconds = 0x3f & (SetByteOrder((unsigned int*)(mpTimecodeData[nTimecodeIndex] + 8)) >> 11); - frame = 0x1f & (SetByteOrder((unsigned int*)(mpTimecodeData[nTimecodeIndex] + 8)) >> 17); - return true; - } - } - return false; -} - -bool CRTPacket::GetTimecodeIRIG(unsigned int nTimecodeIndex, int &year, int &day, int &hours, int &minutes, int &seconds, int &tenths) -{ - if (mnTimecodeCount <= nTimecodeIndex) - { - return false; - } - CRTPacket::ETimecodeType timecodeType; - if (GetTimecodeType(nTimecodeIndex, timecodeType)) - { - if (timecodeType == TimecodeIRIG) - { - year = 0x007f & SetByteOrder((unsigned int*)(mpTimecodeData[nTimecodeIndex] + 4)); - day = 0x01ff & (SetByteOrder((unsigned int*)(mpTimecodeData[nTimecodeIndex] + 4)) >> 7); - hours = 0x001f & SetByteOrder((unsigned int*)(mpTimecodeData[nTimecodeIndex] + 8)); - minutes = 0x003f & (SetByteOrder((unsigned int*)(mpTimecodeData[nTimecodeIndex] + 8)) >> 5); - seconds = 0x003f & (SetByteOrder((unsigned int*)(mpTimecodeData[nTimecodeIndex] + 8)) >> 11); - tenths = 0x000f & (SetByteOrder((unsigned int*)(mpTimecodeData[nTimecodeIndex] + 8)) >> 17); - return true; - } - } - return false; -} - -bool CRTPacket::GetTimecodeCameraTime(unsigned int nTimecodeIndex, unsigned long long &cameraTime) -{ - if (mnTimecodeCount <= nTimecodeIndex) - { - return false; - } - CRTPacket::ETimecodeType timecodeType; - if (GetTimecodeType(nTimecodeIndex, timecodeType)) - { - if (timecodeType == TimecodeCamerTime) - { - cameraTime = ((long long)SetByteOrder((unsigned int*)(mpTimecodeData[nTimecodeIndex] + 4))) << 32 | - (long long)SetByteOrder((unsigned int*)(mpTimecodeData[nTimecodeIndex] + 8)); - return true; - } - } - return false; -} - - -//----------------------------------------------------------- -// Image -//----------------------------------------------------------- -unsigned int CRTPacket::GetImageCameraCount() -{ - return mnImageCameraCount; -} - -unsigned int CRTPacket::GetImageCameraId(unsigned int nCameraIndex) -{ - if (mnImageCameraCount <= nCameraIndex) - { - return 0; - } - return SetByteOrder((unsigned int*)(mpImageData[nCameraIndex])); -} - -bool CRTPacket::GetImageFormat(unsigned int nCameraIndex, EImageFormat &eImageFormat) -{ - if (mnImageCameraCount <= nCameraIndex) - { - return false; - } - eImageFormat = (EImageFormat)SetByteOrder((unsigned int*)(mpImageData[nCameraIndex] + 4)); - - return true; -} - -bool CRTPacket::GetImageSize(unsigned int nCameraIndex, unsigned int &nWidth, unsigned int &nHeight) -{ - if (mnImageCameraCount <= nCameraIndex) - { - return false; - } - nWidth = SetByteOrder((unsigned int*)(mpImageData[nCameraIndex] + 8)); - nHeight = SetByteOrder((unsigned int*)(mpImageData[nCameraIndex] + 12)); - - return true; -} - -bool CRTPacket::GetImageCrop(unsigned int nCameraIndex, float &fCropLeft, float &fCropTop, - float &fCropRight, float &fCropBottom) -{ - if (mnImageCameraCount <= nCameraIndex) - { - return false; - } - fCropLeft = SetByteOrder((float*)(mpImageData[nCameraIndex] + 16)); - fCropTop = SetByteOrder((float*)(mpImageData[nCameraIndex] + 20)); - fCropRight = SetByteOrder((float*)(mpImageData[nCameraIndex] + 24)); - fCropBottom = SetByteOrder((float*)(mpImageData[nCameraIndex] + 28)); - - return true; -} - -unsigned int CRTPacket::GetImageSize(unsigned int nCameraIndex) -{ - if (((mnMajorVersion == 1) && (mnMinorVersion < 8)) || mnImageCameraCount <= nCameraIndex) - { - return 0; - } - - return SetByteOrder((unsigned int*)(mpImageData[nCameraIndex] + 32)); -} - -unsigned int CRTPacket::GetImage(unsigned int nCameraIndex, char* pDataBuf, unsigned int nBufSize) -{ - if (((mnMajorVersion == 1) && (mnMinorVersion < 8)) || mnImageCameraCount <= nCameraIndex) - { - return 0; - } - - unsigned int nSize = SetByteOrder((unsigned int*)(mpImageData[nCameraIndex] + 32)); - - if (nBufSize < nSize) - { - return 0; - } - memcpy(pDataBuf, mpImageData[nCameraIndex] + 36, nSize); - - return nSize; -} - - -//----------------------------------------------------------- -// Analog -//----------------------------------------------------------- -unsigned int CRTPacket::GetAnalogDeviceCount() -{ - return mnAnalogDeviceCount; -} - -unsigned int CRTPacket::GetAnalogDeviceId(unsigned int nDeviceIndex) -{ - if ((mnMajorVersion == 1) && (mnMinorVersion == 0)) - { - return 1; - } - if (mnAnalogDeviceCount <= nDeviceIndex) - { - return 0; - } - return SetByteOrder((unsigned int*)(mpAnalogData[nDeviceIndex])); -} - -unsigned int CRTPacket::GetAnalogChannelCount(unsigned int nDeviceIndex) -{ - char* pData = mpComponentData[ComponentAnalog - 1]; - - if ((mnMajorVersion == 1) && (mnMinorVersion == 0)) - { - return SetByteOrder((unsigned int*)(pData + 8)); - } - if (mnAnalogDeviceCount <= nDeviceIndex) - { - return 0; - } - return SetByteOrder((unsigned int*)(mpAnalogData[nDeviceIndex] + 4)); -} - -unsigned int CRTPacket::GetAnalogSampleCount(unsigned int nDeviceIndex) -{ - if ((mnMajorVersion == 1) && (mnMinorVersion == 0)) - { - return 1; - } - if (mnAnalogDeviceCount <= nDeviceIndex) - { - return 0; - } - return SetByteOrder((unsigned int*)(mpAnalogData[nDeviceIndex] + 8)); -} - -unsigned int CRTPacket::GetAnalogSampleNumber(unsigned int nDeviceIndex) -{ - if ((mnMajorVersion == 1) && (mnMinorVersion == 0)) - { - return GetFrameNumber(); - } - - if (mnAnalogDeviceCount <= nDeviceIndex) - { - return 0; - } - return SetByteOrder((unsigned int*)(mpAnalogData[nDeviceIndex] + 12)); -} - -unsigned int CRTPacket::GetAnalogData(unsigned int nDeviceIndex, float* pDataBuf, unsigned int nBufSize) -{ - unsigned int nSize = 0; - - if (nDeviceIndex < mnAnalogDeviceCount) - { - unsigned int nChannelCount = GetAnalogChannelCount(nDeviceIndex); - - if ((mnMajorVersion == 1) && (mnMinorVersion == 0)) - { - nSize = nChannelCount; - if (nBufSize < nSize || pDataBuf == nullptr) - { - nSize = 0; - } - for (unsigned int i = 0; i < nSize; i++) - { - pDataBuf[i] = (float)SetByteOrder((double*)(mpAnalogData[nDeviceIndex] + i * sizeof(double))); - } - } - else - { - nSize = nChannelCount * GetAnalogSampleCount(nDeviceIndex); - if (nBufSize < nSize || pDataBuf == nullptr) - { - nSize = 0; - } - for (unsigned int i = 0; i < nSize; i++) - { - pDataBuf[i] = (float)SetByteOrder((float*)(mpAnalogData[nDeviceIndex] + 16 + i * sizeof(float))); - } - } - } - - return nSize; -} - -unsigned int CRTPacket::GetAnalogData(unsigned int nDeviceIndex, unsigned int nChannelIndex, float* pDataBuf, unsigned int nBufSize) -{ - unsigned int nSampleCount = 0; - unsigned int nChannelCount = GetAnalogChannelCount(nDeviceIndex); - - if (nDeviceIndex < mnAnalogDeviceCount && nChannelIndex < nChannelCount) - { - if ((mnMajorVersion == 1) && (mnMinorVersion == 0)) - { - if (nBufSize == 0 || pDataBuf == nullptr) - { - nSampleCount = 0; - } - else - { - nSampleCount = 1; - pDataBuf[0] = (float)SetByteOrder((double*)(mpAnalogData[nDeviceIndex] + nChannelIndex * sizeof(double))); - } - } - else - { - nSampleCount = GetAnalogSampleCount(nDeviceIndex); - if (nBufSize < nSampleCount || pDataBuf == nullptr) - { - nSampleCount = 0; - } - for (unsigned int i = 0; i < nSampleCount; i++) - { - pDataBuf[i] = (float)SetByteOrder((float*)(mpAnalogData[nDeviceIndex] + 16 + - nChannelIndex * nSampleCount * sizeof(float) + i * sizeof(float))); - } - } - } - - return nSampleCount; -} - -bool CRTPacket::GetAnalogData(unsigned int nDeviceIndex, unsigned int nChannelIndex, unsigned int nSampleIndex, - float &fAnalogValue) -{ - if (nDeviceIndex < mnAnalogDeviceCount) - { - unsigned int nSampleCount = GetAnalogSampleCount(nDeviceIndex); - - if (GetAnalogChannelCount(nDeviceIndex) > nChannelIndex && - nSampleCount > nSampleIndex) - { - if ((mnMajorVersion == 1) && (mnMinorVersion == 0)) - { - fAnalogValue = (float)SetByteOrder((double*)(mpAnalogData[nDeviceIndex] + nChannelIndex * sizeof(double))); - } - else - { - fAnalogValue = SetByteOrder((float*)(mpAnalogData[nDeviceIndex] + 16 + - (nChannelIndex * nSampleCount + nSampleIndex) * sizeof(float))); - } - if (isnan(fAnalogValue) == 0) - { - return true; - } - } - } - return false; -} - - -//----------------------------------------------------------- -// Analog Single -//----------------------------------------------------------- -unsigned int CRTPacket::GetAnalogSingleDeviceCount() -{ - return mnAnalogSingleDeviceCount; -} - -unsigned int CRTPacket::GetAnalogSingleDeviceId(unsigned int nDeviceIndex) -{ - if (mnAnalogSingleDeviceCount <= nDeviceIndex) - { - return 0; - } - return SetByteOrder((unsigned int*)(mpAnalogSingleData[nDeviceIndex])); -} - -unsigned int CRTPacket::GetAnalogSingleChannelCount(unsigned int nDeviceIndex) -{ - if (mnAnalogSingleDeviceCount <= nDeviceIndex) - { - return 0; - } - return SetByteOrder((unsigned int*)(mpAnalogSingleData[nDeviceIndex] + 4)); -} - -unsigned int CRTPacket::GetAnalogSingleData(unsigned int nDeviceIndex, float* pDataBuf, unsigned int nBufSize) -{ - unsigned int nSize = 0; - - if (nDeviceIndex < mnAnalogSingleDeviceCount) - { - nSize = GetAnalogSingleChannelCount(nDeviceIndex); - if (nBufSize < nSize || pDataBuf == nullptr) - { - nSize = 0; - } - for (unsigned int i = 0; i < nSize; i++) - { - pDataBuf[i] = SetByteOrder((float*)(mpAnalogSingleData[nDeviceIndex] + 8 + i * sizeof(float))); - } - } - - return nSize; -} - -bool CRTPacket::GetAnalogSingleData(unsigned int nDeviceIndex, unsigned int nChannelIndex, float &fValue) -{ - if (nDeviceIndex < mnAnalogSingleDeviceCount) - { - if (nChannelIndex < GetAnalogSingleChannelCount(nDeviceIndex)) - { - fValue = SetByteOrder(((float*)(mpAnalogSingleData[nDeviceIndex] + 8 + nChannelIndex * sizeof(float)))); - return (isnan(fValue) == 0); - } - } - return false; -} - - -//----------------------------------------------------------- -// Force -//----------------------------------------------------------- -unsigned int CRTPacket::GetForcePlateCount() -{ - return mnForcePlateCount; -} - -unsigned int CRTPacket::GetForcePlateId(unsigned int nPlateIndex) -{ - if ((mnMajorVersion == 1 && mnMinorVersion == 0) || mnForcePlateCount <= nPlateIndex) - { - return 0; - } - return SetByteOrder((unsigned int*)(mpForceData[nPlateIndex])); -} - -unsigned int CRTPacket::GetForceCount(unsigned int nPlateIndex) -{ - if (mnForcePlateCount <= nPlateIndex) - { - return 0; - } - if ((mnMajorVersion == 1) && (mnMinorVersion == 0)) - { - return 1; - } - return SetByteOrder((unsigned int*)(mpForceData[nPlateIndex] + 4)); -} - -unsigned int CRTPacket::GetForceNumber(unsigned int nPlateIndex) -{ - if (mnForcePlateCount <= nPlateIndex) - { - return 0; - } - if ((mnMajorVersion == 1) && (mnMinorVersion == 0)) - { - return GetFrameNumber(); - } - return SetByteOrder((unsigned int*)(mpForceData[nPlateIndex] + 8)); -} - -unsigned int CRTPacket::GetForceData(unsigned int nPlateIndex, SForce* pForceBuf, unsigned int nBufSize) -{ - unsigned int nSize = 0; - - if (nPlateIndex < mnForcePlateCount) - { - if ((mnMajorVersion == 1) && (mnMinorVersion == 0)) - { - if (nPlateIndex == 0) - { - for (unsigned int k = 0; k < 9; k++) - { - *(((float*)pForceBuf) + k) = - (float)SetByteOrder((double*)(mpForceData[nPlateIndex] + k * sizeof(double))); - } - nSize = 1; - } - } - else - { - nSize = GetForceCount(nPlateIndex); - if (nBufSize < nSize || pForceBuf == nullptr) - { - nSize = 0; - } - for (unsigned int i = 0; i < nSize; i++) - { - for (unsigned int k = 0; k < 9; k++) - { - *(((float*)&pForceBuf[i]) + k) = - SetByteOrder((float*)(mpForceData[nPlateIndex] + 12 + (k * 4) + i * sizeof(SForce))); - } - } - } - } - return nSize; -} - -bool CRTPacket::GetForceData(unsigned int nPlateIndex, unsigned int nForceIndex, SForce &sForce) -{ - if (nPlateIndex < mnForcePlateCount) - { - if ((mnMajorVersion == 1) && (mnMinorVersion == 0)) - { - if (nPlateIndex == 0 && nForceIndex == 0) - { - for (unsigned int k = 0; k < 9; k++) - { - *(((float*)&sForce) + k) = - (float)SetByteOrder((double*)(mpForceData[nPlateIndex] + k * sizeof(double))); - - // Not a valid force if one of the values is not a valid float. - if (isnan(*(((float*)&sForce) + k)) != 0) - { - return false; - } - } - return true; - } - } - else - { - if (nForceIndex < GetForceCount(nPlateIndex)) - { - for (unsigned int k = 0; k < 9; k++) - { - *(((float*)&sForce) + k) = - SetByteOrder((float*)(mpForceData[nPlateIndex] + 12 + k * sizeof(float) + nForceIndex * sizeof(SForce))); - - // Not a valid force if one of the values is not a valid float. - if (isnan(*(((float*)&sForce) + k)) != 0) - { - return false; - } - } - return true; - } - } - } - return false; -} - - -//----------------------------------------------------------- -// Skeleton -//----------------------------------------------------------- -unsigned int CRTPacket::GetSkeletonCount() -{ - return mSkeletonCount; -} - -unsigned int CRTPacket::GetSkeletonSegmentCount(unsigned int nSkeletonIndex) -{ - if (mSkeletonCount <= nSkeletonIndex) - { - return 0; - } - return SetByteOrder((unsigned int*)(mpSkeletonData[nSkeletonIndex])); -} - -bool CRTPacket::GetSkeletonSegments(unsigned int nSkeletonIndex, SSkeletonSegment* segmentBuffer, unsigned int nBufSize) -{ - if (mSkeletonCount <= nSkeletonIndex) - { - return false; - } - - unsigned int segmentCount = GetSkeletonSegmentCount(nSkeletonIndex); - if (segmentCount == 0) - { - return false; - } - - if (nBufSize < segmentCount * 32 || segmentBuffer == nullptr) - { - segmentCount = 0; - return false; - } - - if (mbBigEndian) - { - for (unsigned int i = 0; i < segmentCount; i++) - { - segmentBuffer[i].id = SetByteOrder((unsigned int*)(mpSkeletonData[nSkeletonIndex] + 4 + i * 32)); - segmentBuffer[i].positionX = SetByteOrder((float*)(mpSkeletonData[nSkeletonIndex] + 8 + i * 32)); - segmentBuffer[i].positionY = SetByteOrder((float*)(mpSkeletonData[nSkeletonIndex] + 12 + i * 32)); - segmentBuffer[i].positionZ = SetByteOrder((float*)(mpSkeletonData[nSkeletonIndex] + 16 + i * 32)); - segmentBuffer[i].rotationX = SetByteOrder((float*)(mpSkeletonData[nSkeletonIndex] + 20 + i * 32)); - segmentBuffer[i].rotationY = SetByteOrder((float*)(mpSkeletonData[nSkeletonIndex] + 24 + i * 32)); - segmentBuffer[i].rotationZ = SetByteOrder((float*)(mpSkeletonData[nSkeletonIndex] + 28 + i * 32)); - segmentBuffer[i].rotationW = SetByteOrder((float*)(mpSkeletonData[nSkeletonIndex] + 32 + i * 32)); - } - } - else - { - memcpy(segmentBuffer, mpSkeletonData[nSkeletonIndex] + 4, sizeof(SSkeletonSegment) * segmentCount); - } - return true; -} - -bool CRTPacket::GetSkeletonSegment(unsigned int nSkeletonIndex, unsigned segmentIndex, SSkeletonSegment &segment) -{ - if (mSkeletonCount <= nSkeletonIndex) - { - return false; - } - - unsigned int segmentCount = GetSkeletonSegmentCount(nSkeletonIndex); - if (segmentCount == 0) - { - return false; - } - - if (segmentIndex >= segmentCount) - { - return false; - } - - if (mbBigEndian) - { - segment.id = SetByteOrder((unsigned int*)(mpSkeletonData[nSkeletonIndex] + 4 + 32 * segmentIndex)); - segment.positionX = SetByteOrder((float*)(mpSkeletonData[nSkeletonIndex] + 8 + 32 * segmentIndex)); - segment.positionY = SetByteOrder((float*)(mpSkeletonData[nSkeletonIndex] + 12 + 32 * segmentIndex)); - segment.positionZ = SetByteOrder((float*)(mpSkeletonData[nSkeletonIndex] + 16 + 32 * segmentIndex)); - segment.rotationX = SetByteOrder((float*)(mpSkeletonData[nSkeletonIndex] + 20 + 32 * segmentIndex)); - segment.rotationY = SetByteOrder((float*)(mpSkeletonData[nSkeletonIndex] + 24 + 32 * segmentIndex)); - segment.rotationZ = SetByteOrder((float*)(mpSkeletonData[nSkeletonIndex] + 28 + 32 * segmentIndex)); - segment.rotationW = SetByteOrder((float*)(mpSkeletonData[nSkeletonIndex] + 32 + 32 * segmentIndex)); - } - else - { - memcpy(&segment, mpSkeletonData[nSkeletonIndex] + 4 + 32 * segmentIndex, sizeof(SSkeletonSegment)); - } - - return true; -} - - -//----------------------------------------------------------- -// Force Single -//----------------------------------------------------------- -unsigned int CRTPacket::GetForceSinglePlateCount() -{ - return mnForceSinglePlateCount; -} - -unsigned int CRTPacket::GetForceSinglePlateId(unsigned int nPlateIndex) -{ - if ((mnMajorVersion == 1 && mnMinorVersion == 0) || mnForceSinglePlateCount <= nPlateIndex) - { - return 0; - } - return SetByteOrder((unsigned int*)(mpForceSingleData[nPlateIndex])); -} - -bool CRTPacket::GetForceSingleData(unsigned int nPlateIndex, SForce &sForce) -{ - if (nPlateIndex < mnForceSinglePlateCount) - { - for (unsigned int k = 0; k < 9; k++) - { - *(((float*)&sForce) + k) = - SetByteOrder((float*)(mpForceSingleData[nPlateIndex] + 4 + k * sizeof(float))); - - // Not a valid force if one of the values is not a valid float. - if (isnan(*(((float*)&sForce) + k)) != 0) - { - return false; - } - } - return true; - } - - return false; -} - -float CRTPacket::SetByteOrder(float* pfData) -{ - unsigned int nTmp; - - if (mbBigEndian) - { - nTmp = ntohl(*((unsigned int*)pfData)); - return *((float*)&nTmp); - } - return *pfData; -} // SetByteOrder - -double CRTPacket::SetByteOrder(double* pfData) -{ - unsigned long long nTmp; - - if (mbBigEndian) - { - nTmp = (((unsigned long long)(ntohl((long)*((unsigned long long*)pfData))) << 32) + ntohl(*((unsigned long long*)pfData) >> 32)); - return *((double*)&nTmp); - } - return *pfData; -} // SetByteOrder - -short CRTPacket::SetByteOrder(short* pnData) -{ - if (mbBigEndian) - { - return ntohs(*pnData); - } - return *pnData; -} // SetByteOrder - -unsigned short CRTPacket::SetByteOrder(unsigned short* pnData) -{ - if (mbBigEndian) - { - return ntohs(*pnData); - } - return *pnData; -} // SetByteOrder - -long CRTPacket::SetByteOrder(long* pnData) -{ - if (mbBigEndian) - { - return ntohl(*pnData); - } - return *pnData; -} // SetByteOrder - -int CRTPacket::SetByteOrder(int* pnData) -{ - if (mbBigEndian) - { - return ntohl(*pnData); - } - return *pnData; -} // SetByteOrder - -unsigned int CRTPacket::SetByteOrder(unsigned int* pnData) -{ - if (mbBigEndian) - { - return ntohl(*pnData); - } - return *pnData; -} // SetByteOrder - -long long CRTPacket::SetByteOrder(long long* pnData) -{ - if (mbBigEndian) - { - return ((unsigned long long)(ntohl((long)*pnData)) << 32) + ntohl(*pnData >> 32); - } - return *pnData; -} // SetByteOrder - -unsigned long long CRTPacket::SetByteOrder(unsigned long long* pnData) -{ - if (mbBigEndian) - { - return ((unsigned long long)(ntohl((long)*pnData)) << 32) + ntohl(*pnData >> 32); - } - return *pnData; -} // SetByteOrder \ No newline at end of file diff --git a/navigation/qualisys_motion_capture/mocap_qualisys/include/mocap_qualisys/RTPacket.h b/navigation/qualisys_motion_capture/mocap_qualisys/include/mocap_qualisys/RTPacket.h deleted file mode 100644 index 9cd722edf..000000000 --- a/navigation/qualisys_motion_capture/mocap_qualisys/include/mocap_qualisys/RTPacket.h +++ /dev/null @@ -1,308 +0,0 @@ -#ifndef RTPACKET_H -#define RTPACKET_H - -#include -#include - -#ifdef EXPORT_DLL - #define DLL_EXPORT __declspec(dllexport) -#else - #define DLL_EXPORT -#endif - - -#define MAJOR_VERSION 1 -#define MINOR_VERSION 20 - -#define MAX_CAMERA_COUNT 256 -#define MAX_ANALOG_DEVICE_COUNT 64 -#define MAX_FORCE_PLATE_COUNT 64 -#define MAX_GAZE_VECTOR_COUNT 64 -#define MAX_TIMECODE_COUNT 3 -#define MAX_SKELETON_COUNT 10 - - -class DLL_EXPORT CRTPacket -{ -public: - enum EPacketType - { - PacketError = 0, - PacketCommand = 1, - PacketXML = 2, - PacketData = 3, - PacketNoMoreData = 4, - PacketC3DFile = 5, - PacketEvent = 6, - PacketDiscover = 7, - PacketQTMFile = 8, - PacketNone = 9 - }; - - enum EComponentType - { - Component3d = 1, - Component3dNoLabels = 2, - ComponentAnalog = 3, - ComponentForce = 4, - Component6d = 5, - Component6dEuler = 6, - Component2d = 7, - Component2dLin = 8, - Component3dRes = 9, - Component3dNoLabelsRes = 10, - Component6dRes = 11, - Component6dEulerRes = 12, - ComponentAnalogSingle = 13, - ComponentImage = 14, - ComponentForceSingle = 15, - ComponentGazeVector = 16, - ComponentTimecode = 17, - ComponentSkeleton = 18, - ComponentNone = 19 - }; - - enum EImageFormat - { - FormatRawGrayscale = 0, - FormatRawBGR = 1, - FormatJPG = 2, - FormatPNG = 3 - }; - - enum EEvent - { - EventConnected = 1, - EventConnectionClosed = 2, - EventCaptureStarted = 3, - EventCaptureStopped = 4, - EventCaptureFetchingFinished = 5, // Not used in version 1.10 and later - EventCalibrationStarted = 6, - EventCalibrationStopped = 7, - EventRTfromFileStarted = 8, - EventRTfromFileStopped = 9, - EventWaitingForTrigger = 10, - EventCameraSettingsChanged = 11, - EventQTMShuttingDown = 12, - EventCaptureSaved = 13, - EventReprocessingStarted = 14, - EventReprocessingStopped = 15, - EventTrigger = 16, - EventNone = 17 // Must be the last. Not actually an event. Just used to cont number of events. - }; - - enum ETimecodeType - { - TimecodeSMPTE = 0, - TimecodeIRIG = 1, - TimecodeCamerTime = 2 - }; - - struct SForce - { - float fForceX; - float fForceY; - float fForceZ; - float fMomentX; - float fMomentY; - float fMomentZ; - float fApplicationPointX; - float fApplicationPointY; - float fApplicationPointZ; - }; - - struct SGazeVector - { - float fX; - float fY; - float fZ; - float fPosX; - float fPosY; - float fPosZ; - }; - - struct SSkeletonSegment - { - unsigned int id; - float positionX; - float positionY; - float positionZ; - float rotationX; - float rotationY; - float rotationZ; - float rotationW; - }; - -public: - CRTPacket(int nMajorVersion = MAJOR_VERSION, int nMinorVersion = MINOR_VERSION, bool bBigEndian = false); - void GetVersion(unsigned int &nMajorVersion, unsigned int &nMinorVersion); - void SetVersion(unsigned int nMajorVersion, unsigned int nMinorVersion); - bool GetEndianness(); - void SetEndianness(bool bBigEndian); - void ClearData(); - void SetData(char* ptr); - void GetData(char* &ptr, unsigned int &nSize); - - unsigned int GetSize(); - EPacketType GetType(); - unsigned long long GetTimeStamp(); - unsigned int GetFrameNumber(); - static unsigned int GetSize(char* pData, bool bBigEndian = false); - static EPacketType GetType(char* pData, bool bBigEndian = false); - static unsigned long long GetTimeStamp(char* pData, bool bBigEndian = false); - static unsigned int GetFrameNumber(char* pData, bool bBigEndian = false); - - unsigned int GetComponentCount(); - unsigned int GetComponentSize(EComponentType eComponent); - - char* GetErrorString(); - char* GetCommandString(); - static char* GetCommandString(char* pData, bool bBigEndian = false); - char* GetXMLString(); - bool GetEvent(EEvent &eEvent); - static bool GetEvent(EEvent &eEvent, char* pData, bool bBigEndian = false); - short GetDiscoverResponseBasePort(); - static short GetDiscoverResponseBasePort(char* pData, bool bBigEndian = false); - - unsigned short GetDropRate(); - unsigned short GetOutOfSyncRate(); - - unsigned int Get2DCameraCount(); - unsigned int Get2DMarkerCount(unsigned int nCameraIndex); - unsigned char Get2DStatusFlags(unsigned int nCameraIndex); - bool Get2DMarker(unsigned int nCameraIndex, unsigned int nMarkerIndex, unsigned int &nX, - unsigned int &nY, unsigned short &nXDiameter, unsigned short & nYDiameter); - - unsigned int Get2DLinCameraCount(); - unsigned int Get2DLinMarkerCount(unsigned int nCameraIndex); - unsigned char Get2DLinStatusFlags(unsigned int nCameraIndex); - bool Get2DLinMarker(unsigned int nCameraIndex, unsigned int nMarkerIndex, unsigned int &nX, - unsigned int &nY, unsigned short &nXDiameter, unsigned short & nYDiameter); - - unsigned int Get3DMarkerCount(); - bool Get3DMarker(unsigned int nMarkerIndex, float &fX, float &fY, float &fZ); - - unsigned int Get3DResidualMarkerCount(); - bool Get3DResidualMarker(unsigned int nMarkerIndex, float &fX, float &fY, float &fZ, - float &fResidual); - - unsigned int Get3DNoLabelsMarkerCount(); - bool Get3DNoLabelsMarker(unsigned int nMarkerIndex, float &fX, float &fY, float &fZ, - unsigned int &nId); - - unsigned int Get3DNoLabelsResidualMarkerCount(); - bool Get3DNoLabelsResidualMarker(unsigned int nMarkerIndex, float &fX, float &fY, float &fZ, - unsigned int &nId, float &fResidual); - - - unsigned int Get6DOFBodyCount(); - bool Get6DOFBody(unsigned int nBodyIndex, float &fX, float &fY, float &fZ, float afRotMatrix[9]); - - unsigned int Get6DOFResidualBodyCount(); - bool Get6DOFResidualBody(unsigned int nBodyIndex, float &fX, float &fY, float &fZ, - float afRotMatrix[9], float &fResidual); - - unsigned int Get6DOFEulerBodyCount(); - bool Get6DOFEulerBody(unsigned int nBodyIndex, float &fX, float &fY, float &fZ, - float &fAng1, float &fAng2, float &fAng3); - - unsigned int Get6DOFEulerResidualBodyCount(); - bool Get6DOFEulerResidualBody(unsigned int nBodyIndex, float &fX, float &fY, float &fZ, - float &fAng1, float &fAng2, float &fAng3, float &fResidual); - - unsigned int GetGazeVectorCount(); - unsigned int GetGazeVectorSampleCount(unsigned int nVectorIndex); - unsigned int GetGazeVectorSampleNumber(unsigned int nVectorIndex); // Returns 0 if no sample was found. - bool GetGazeVector(unsigned int nVectorIndex, unsigned int nSampleIndex, SGazeVector &nGazeVector); - bool GetGazeVector(unsigned int nVectorIndex, SGazeVector* pGazeVectorBuf, unsigned int nBufSize); - - unsigned int GetTimecodeCount(); - bool GetTimecodeType(unsigned int nTimecodeIndex, CRTPacket::ETimecodeType &timecodeType); - bool GetTimecodeSMPTE(unsigned int nTimecodeIndex, int &hours, int &minutes, int &seconds, int &frame); - bool GetTimecodeIRIG(unsigned int nTimecodeIndex, int &year, int &day, int &hours, int &minutes, int &seconds, int &tenths); - bool GetTimecodeCameraTime(unsigned int nTimecodeIndex, unsigned long long &cameraTime); - - unsigned int GetImageCameraCount(); - unsigned int GetImageCameraId(unsigned int nCameraIndex); - bool GetImageFormat(unsigned int nCameraIndex, EImageFormat &eImageFormat); - bool GetImageSize(unsigned int nCameraIndex, unsigned int &nWidth, unsigned int &nHeight); - bool GetImageCrop(unsigned int nCameraIndex, float &fCropLeft, float &fCropTop, - float &fCropRight, float &fCropBottom); - unsigned int GetImageSize(unsigned int nCameraIndex); - unsigned int GetImage(unsigned int nCameraIndex, char* pDataBuf, unsigned int nBufSize); - - unsigned int GetAnalogDeviceCount(); - unsigned int GetAnalogDeviceId(unsigned int nDeviceIndex); - unsigned int GetAnalogChannelCount(unsigned int nDeviceIndex); - unsigned int GetAnalogSampleCount(unsigned int nDeviceIndex); - unsigned int GetAnalogSampleNumber(unsigned int nDeviceIndex); // Returns 0 if no sample was found. - unsigned int GetAnalogData(unsigned int nDeviceIndex, float* pDataBuf, unsigned int nBufSize); - unsigned int GetAnalogData(unsigned int nDeviceIndex, unsigned int nChannelIndex, float* pDataBuf, unsigned int nBufSize); - bool GetAnalogData(unsigned int nDeviceIndex, unsigned int nChannelIndex, - unsigned int nSampleIndex, float &fAnalogValue); - - unsigned int GetAnalogSingleDeviceCount(); - unsigned int GetAnalogSingleDeviceId(unsigned int nDeviceIndex); - unsigned int GetAnalogSingleChannelCount(unsigned int nDeviceIndex); - unsigned int GetAnalogSingleData(unsigned int nDeviceIndex, float* pDataBuf, unsigned int nBufSize); - bool GetAnalogSingleData(unsigned int nDeviceIndex, unsigned int nChannelIndex, float &fValue); - - unsigned int GetForcePlateCount(); - unsigned int GetForcePlateId(unsigned int nPlateIndex); - unsigned int GetForceCount(unsigned int nPlateIndex); - unsigned int GetForceNumber(unsigned int nPlateIndex);// Returns 0 if no force was found. - unsigned int GetForceData(unsigned int nPlateIndex, SForce* pForceBuf, unsigned int nBufSize); - bool GetForceData(unsigned int nPlateIndex, unsigned int nForceIndex, SForce &sForce); - - unsigned int GetForceSinglePlateCount(); - unsigned int GetForceSinglePlateId(unsigned int nPlateIndex); - bool GetForceSingleData(unsigned int nPlateIndex, SForce &pForce); - - unsigned int GetSkeletonCount(); - unsigned int GetSkeletonSegmentCount(unsigned int nSkeletonIndex); - bool GetSkeletonSegments(unsigned int nSkeletonIndex, SSkeletonSegment* segmentBuf, unsigned int nBufSize); - bool GetSkeletonSegment(unsigned int nSkeletonIndex, unsigned segmentIndex, SSkeletonSegment &segment); - -private: - float SetByteOrder(float* pfData); - double SetByteOrder(double* pfData); - short SetByteOrder(short* pnData); - unsigned short SetByteOrder(unsigned short* pnData); - long SetByteOrder(long* pnData); - int SetByteOrder(int* pnData); - unsigned int SetByteOrder(unsigned int* pnData); - long long SetByteOrder(long long* pnData); - unsigned long long SetByteOrder(unsigned long long* pnData); - -private: - char* mpData; - char* mpComponentData[ComponentNone]; - char* mp2DData[MAX_CAMERA_COUNT]; - char* mp2DLinData[MAX_CAMERA_COUNT]; - char* mpImageData[MAX_CAMERA_COUNT]; - char* mpAnalogData[MAX_ANALOG_DEVICE_COUNT]; - char* mpAnalogSingleData[MAX_ANALOG_DEVICE_COUNT]; - char* mpForceData[MAX_FORCE_PLATE_COUNT]; - char* mpForceSingleData[MAX_FORCE_PLATE_COUNT]; - char* mpGazeVectorData[MAX_GAZE_VECTOR_COUNT]; - char* mpTimecodeData[MAX_TIMECODE_COUNT]; - char* mpSkeletonData[MAX_SKELETON_COUNT]; - unsigned int mnComponentCount; - EComponentType meComponentType; - unsigned int mn2DCameraCount; - unsigned int mn2DLinCameraCount; - unsigned int mnImageCameraCount; - unsigned int mnAnalogDeviceCount; - unsigned int mnAnalogSingleDeviceCount; - unsigned int mnForcePlateCount; - unsigned int mnForceSinglePlateCount; - unsigned int mnGazeVectorCount; - unsigned int mnTimecodeCount; - unsigned int mSkeletonCount; - int mnMajorVersion; - int mnMinorVersion; - bool mbBigEndian; -}; // RTPacket - - -#endif // RTPACKET_H \ No newline at end of file diff --git a/navigation/qualisys_motion_capture/mocap_qualisys/include/mocap_qualisys/RTProtocol.cpp b/navigation/qualisys_motion_capture/mocap_qualisys/include/mocap_qualisys/RTProtocol.cpp deleted file mode 100644 index d8313ff29..000000000 --- a/navigation/qualisys_motion_capture/mocap_qualisys/include/mocap_qualisys/RTProtocol.cpp +++ /dev/null @@ -1,5458 +0,0 @@ -#define _CRT_SECURE_NO_WARNINGS -#define NOMINMAX - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "RTProtocol.h" -#include "Markup.h" -#include "Network.h" -#include -#ifdef _WIN32 -#include -// import the internet protocol helper library. -#pragma comment(lib, "IPHLPAPI.lib") -#else -#include - -#endif - -namespace -{ - inline void ToLower(std::string& str) - { - std::transform(str.begin(), str.end(), str.begin(), [](int c) - { - return std::tolower(c); - }); - } - - inline void RemoveInvalidChars(std::string& str) - { - auto isInvalidChar = [](int c) -> int - { - // iscntrl: control codes(NUL, etc.), '\t', '\n', '\v', '\f', '\r', backspace (DEL) - // isspace: some common checks but also 0x20 (SPACE) - // return != 0 --> invalid char - return std::iscntrl(c) + std::isspace(c); - }; - str.erase(std::remove_if(str.begin(), str.end(), isInvalidChar), str.end()); - } - -} - -unsigned int CRTProtocol::GetSystemFrequency() const -{ - return msGeneralSettings.nCaptureFrequency; -} - -CRTProtocol::CRTProtocol() -{ - mpoNetwork = new CNetwork(); - mpoRTPacket = nullptr; - meLastEvent = CRTPacket::EventCaptureStopped; - meState = CRTPacket::EventCaptureStopped; - mnMajorVersion = 1; - mnMinorVersion = 0; - mbBigEndian = false; - maErrorStr[0] = 0; - mnBroadcastPort = 0; - mpFileBuffer = nullptr; - mDataBuffSize = 65535; - maDataBuff = new char[mDataBuffSize]; - mbIsMaster = false; -} // CRTProtocol - - -CRTProtocol::~CRTProtocol() -{ - if (mpoNetwork) - { - delete mpoNetwork; - mpoNetwork = nullptr; - } - if (mpoRTPacket) - { - delete mpoRTPacket; - mpoRTPacket = nullptr; - } - if (maDataBuff) - { - delete maDataBuff; - maDataBuff = nullptr; - } - -} // ~CRTProtocol - - -bool CRTProtocol::Connect(const char* pServerAddr, unsigned short nPort, unsigned short* pnUDPServerPort, - int nMajorVersion, int nMinorVersion, bool bBigEndian) -{ - CRTPacket::EPacketType eType; - char tTemp[100]; - char pResponseStr[256]; - - mbBigEndian = bBigEndian; - mbIsMaster = false; - - mnMajorVersion = 1; - if ((nMajorVersion == 1) && (nMinorVersion == 0)) - { - mnMinorVersion = 0; - } - else - { - mnMinorVersion = 1; - if (mbBigEndian) - { - nPort += 2; - } - else - { - nPort += 1; - } - } - - if (mpoRTPacket) - { - delete mpoRTPacket; - } - mpoRTPacket = new CRTPacket(nMajorVersion, nMinorVersion, bBigEndian); - - if (mpoRTPacket == nullptr) - { - strcpy(maErrorStr, "Could not allocate data packet."); - return false; - } - - if (mpoNetwork->Connect(pServerAddr, nPort)) - { - if (pnUDPServerPort != nullptr) - { - if (mpoNetwork->CreateUDPSocket(*pnUDPServerPort) == false) - { - sprintf(maErrorStr, "CreateUDPSocket failed. %s", mpoNetwork->GetErrorString()); - Disconnect(); - return false; - } - } - - // Welcome message - auto received = ReceiveRTPacket(eType, true); - if (received > 0) - { - if (eType == CRTPacket::PacketError) - { - strcpy(maErrorStr, mpoRTPacket->GetErrorString()); - Disconnect(); - return false; - } - else if (eType == CRTPacket::PacketCommand) - { - const std::string welcomeMessage("QTM RT Interface connected"); - if (strncmp(welcomeMessage.c_str(), mpoRTPacket->GetCommandString(), welcomeMessage.size()) == 0) - { - // Set protocol version - if (SetVersion(nMajorVersion, nMinorVersion)) - { - // Set byte order. - // Unless we use protocol version 1.0, we have set the byte order by selecting the correct port. - - if ((mnMajorVersion == 1) && (mnMinorVersion == 0)) - { - if (mbBigEndian) - { - strcpy(tTemp, "ByteOrder BigEndian"); - } - else - { - strcpy(tTemp, "ByteOrder LittleEndian"); - } - - if (SendCommand(tTemp, pResponseStr)) - { - return true; - } - else - { - strcpy(maErrorStr, "Set byte order failed."); - } - } - else - { - GetState(meState, true); - return true; - } - } - Disconnect(); - return false; - } - } - } - } - else - { - if (mpoNetwork->GetError() == 10061) - { - strcpy(maErrorStr, "Check if QTM is running on target machine."); - } - else - { - strcpy(maErrorStr, mpoNetwork->GetErrorString()); - } - } - Disconnect(); - return false; -} // Connect - - -unsigned short CRTProtocol::GetUdpServerPort() -{ - if (mpoNetwork) - { - return mpoNetwork->GetUdpServerPort(); - } - return 0; -} - - -void CRTProtocol::Disconnect() -{ - mpoNetwork->Disconnect(); - mnBroadcastPort = 0; - if (mpoRTPacket) - { - delete mpoRTPacket; - mpoRTPacket = nullptr; - } - mbIsMaster = false; -} // Disconnect - - -bool CRTProtocol::Connected() const -{ - return mpoNetwork->Connected(); -} - - -bool CRTProtocol::SetVersion(int nMajorVersion, int nMinorVersion) -{ - char tTemp[256]; - char pResponseStr[256]; - - sprintf(tTemp, "Version %u.%u", nMajorVersion, nMinorVersion); - - if (SendCommand(tTemp, pResponseStr)) - { - sprintf(tTemp, "Version set to %u.%u", nMajorVersion, nMinorVersion); - - if (strcmp(pResponseStr, tTemp) == 0) - { - mnMajorVersion = nMajorVersion; - mnMinorVersion = nMinorVersion; - mpoRTPacket->SetVersion(mnMajorVersion, mnMinorVersion); - return true; - } - if (pResponseStr) - { - sprintf(maErrorStr, "%s.", pResponseStr); - } - else - { - strcpy(maErrorStr, "Set Version failed."); - } - } - else if (pResponseStr) - { - sprintf(maErrorStr, "Set Version to %s failed: %s.", tTemp, pResponseStr); - } - else - { - strcpy(tTemp, maErrorStr); - sprintf(maErrorStr, "Send Version failed. %s.", tTemp); - } - return false; -} - -bool CRTProtocol::GetVersion(unsigned int &nMajorVersion, unsigned int &nMinorVersion) -{ - if (!Connected()) - { - strcpy(maErrorStr, "Get version failed. Not connected"); - return false; - } - nMajorVersion = mnMajorVersion; - nMinorVersion = mnMinorVersion; - return true; -} - - - -bool CRTProtocol::GetQTMVersion(char* pVersion, unsigned int nVersionLen) -{ - if (SendCommand("QTMVersion", pVersion, nVersionLen)) - { - return true; - } - strcpy(maErrorStr, "Get QTM Version failed."); - return false; -} - - -bool CRTProtocol::GetByteOrder(bool &bBigEndian) -{ - char pResponseStr[256]; - - if (SendCommand("ByteOrder", pResponseStr)) - { - bBigEndian = (strcmp(pResponseStr, "Byte order is big endian") == 0); - return true; - } - strcpy(maErrorStr, "Get Byte order failed."); - return false; -} - - -bool CRTProtocol::CheckLicense(const char* pLicenseCode) -{ - char tTemp[100]; - char pResponseStr[256]; - - if (strlen(pLicenseCode) <= 85) - { - sprintf(tTemp, "CheckLicense %s", pLicenseCode); - - if (SendCommand(tTemp, pResponseStr)) - { - if (strcmp(pResponseStr, "License pass") == 0) - { - return true; - } - strcpy(maErrorStr, "Wrong license code."); - } - else - { - strcpy(maErrorStr, "CheckLicense failed."); - } - } - else - { - strcpy(maErrorStr, "License code too long."); - } - return false; -} - - -bool CRTProtocol::DiscoverRTServer(unsigned short nServerPort, bool bNoLocalResponses, unsigned short nDiscoverPort) -{ - char pData[10]; - SDiscoverResponse sResponse; - - if (mnBroadcastPort == 0) - { - if (!mpoNetwork->CreateUDPSocket(nServerPort, true)) - { - strcpy(maErrorStr, mpoNetwork->GetErrorString()); - return false; - } - mnBroadcastPort = nServerPort; - } - else - { - nServerPort = mnBroadcastPort; - } - - *((unsigned int*)pData) = (unsigned int)10; - *((unsigned int*)(pData + 4)) = (unsigned int)CRTPacket::PacketDiscover; - *((unsigned short*)(pData + 8)) = htons(nServerPort); - - if (mpoNetwork->SendUDPBroadcast(pData, 10, nDiscoverPort)) - { - mvsDiscoverResponseList.clear(); - - int nReceived; - do - { - unsigned int nAddr = 0; - nReceived = mpoNetwork->Receive(maDataBuff, mDataBuffSize, false, 100000, &nAddr); - - if (nReceived != -1 && nReceived > 8) - { - if (CRTPacket::GetType(maDataBuff) == CRTPacket::PacketCommand) - { - char* response = CRTPacket::GetCommandString(maDataBuff); - sResponse.nAddr = nAddr; - sResponse.nBasePort = CRTPacket::GetDiscoverResponseBasePort(maDataBuff); - - if (response && (!bNoLocalResponses || !mpoNetwork->IsLocalAddress(nAddr))) - { - strcpy(sResponse.message, response); - mvsDiscoverResponseList.push_back(sResponse); - } - } - } - } while (nReceived != -1 && nReceived > 8); // Keep reading until no more responses. - - return true; - } - return false; -} - - -int CRTProtocol::GetNumberOfDiscoverResponses() -{ - return (int)mvsDiscoverResponseList.size(); -} - - -bool CRTProtocol::GetDiscoverResponse(unsigned int nIndex, unsigned int &nAddr, unsigned short &nBasePort, std::string& message) -{ - if (nIndex < mvsDiscoverResponseList.size()) - { - nAddr = mvsDiscoverResponseList[nIndex].nAddr; - nBasePort = mvsDiscoverResponseList[nIndex].nBasePort; - message = mvsDiscoverResponseList[nIndex].message; - return true; - } - return false; -} - -bool CRTProtocol::GetCurrentFrame(const char* components) -{ - char pCommandStr[256]; - strcpy(pCommandStr, "GetCurrentFrame "); - strcat(pCommandStr, components); - - if (SendCommand(pCommandStr)) - { - return true; - } - strcpy(maErrorStr, "GetCurrentFrame failed."); - - return false; -} - - -bool CRTProtocol::GetCurrentFrame(unsigned int nComponentType, const SComponentOptions& componentOptions) -{ - char components[256]; - - if (GetComponentString(components, nComponentType, componentOptions)) - { - return GetCurrentFrame(components); - } - else - { - strcpy(maErrorStr, "DataComponent missing."); - } - return false; -} - - -bool CRTProtocol::StreamFrames(EStreamRate eRate, unsigned int nRateArg, unsigned short nUDPPort, const char* pUDPAddr, const char* components) -{ - std::ostringstream commandString; - - if (eRate == RateFrequencyDivisor) - { - commandString << "StreamFrames FrequencyDivisor: " << nRateArg << " "; - } - else if (eRate == RateFrequency) - { - commandString << "StreamFrames Frequency:" << nRateArg << " "; - } - else if (eRate == RateAllFrames) - { - commandString << "StreamFrames AllFrames "; - } - else - { - commandString << "No valid rate."; - return false; - } - - if (nUDPPort > 0) - { - if (pUDPAddr != nullptr && strlen(pUDPAddr) > 64) - { - strcpy(maErrorStr, "UDP address string too long."); - return false; - } - commandString << "UDP"; - if (pUDPAddr != nullptr) - { - commandString << ":" << pUDPAddr; - } - commandString << ":" << nUDPPort << " "; - } - - commandString << components; - - if (SendCommand(commandString.str().c_str())) - { - return true; - } - strcpy(maErrorStr, "StreamFrames failed."); - - return false; -} - -bool CRTProtocol::StreamFrames(EStreamRate eRate, unsigned int nRateArg, unsigned short nUDPPort, const char* pUDPAddr, - unsigned int nComponentType, const SComponentOptions& componentOptions) -{ - char components[256]; - - if (GetComponentString(components, nComponentType, componentOptions)) - { - return StreamFrames(eRate, nRateArg, nUDPPort, pUDPAddr, components); - } - else - { - strcpy(maErrorStr, "DataComponent missing."); - } - - return false; -} - - -bool CRTProtocol::StreamFramesStop() -{ - if (SendCommand("StreamFrames Stop")) - { - return true; - } - strcpy(maErrorStr, "StreamFrames Stop failed."); - return false; -} - - -bool CRTProtocol::GetState(CRTPacket::EEvent &eEvent, bool bUpdate, int nTimeout) -{ - CRTPacket::EPacketType eType; - - if (bUpdate) - { - bool result; - if (mnMajorVersion > 1 || mnMinorVersion > 9) - { - result = SendCommand("GetState"); - } - else - { - result = SendCommand("GetLastEvent"); - } - if (result) - { - int nReceived; - do - { - nReceived = ReceiveRTPacket(eType, false, nTimeout); - if (nReceived > 0) - { - if (mpoRTPacket->GetEvent(eEvent)) - { - return true; - } - } - } while (nReceived > 0); - } - strcpy(maErrorStr, "GetLastEvent failed."); - } - else - { - eEvent = meState; - return true; - } - return false; -} - - -bool CRTProtocol::GetCapture(const char* pFileName, bool bC3D) -{ - CRTPacket::EPacketType eType; - char pResponseStr[256]; - - mpFileBuffer = fopen(pFileName, "wb"); - if (mpFileBuffer != nullptr) - { - if (bC3D) - { - // C3D file - if (SendCommand((mnMajorVersion > 1 || mnMinorVersion > 7) ? "GetCaptureC3D" : "GetCapture", pResponseStr)) - { - if (strcmp(pResponseStr, "Sending capture") == 0) - { - if (ReceiveRTPacket(eType, true, 5000000) > 0) // Wait for C3D file in 5 seconds. - { - if (eType == CRTPacket::PacketC3DFile) - { - if (mpFileBuffer != nullptr) - { - fclose(mpFileBuffer); - return true; - } - strcpy(maErrorStr, "Writing C3D file failed."); - } - else - { - strcpy(maErrorStr, "Wrong packet type received."); - } - } - else - { - strcpy(maErrorStr, "No packet received."); - } - } - else - { - sprintf(maErrorStr, "%s failed.", (mnMajorVersion > 1 || mnMinorVersion > 7) ? "GetCaptureC3D" : "GetCapture"); - } - } - else - { - sprintf(maErrorStr, "%s failed.", (mnMajorVersion > 1 || mnMinorVersion > 7) ? "GetCaptureC3D" : "GetCapture"); - } - } - else - { - // QTM file - if (SendCommand("GetCaptureQTM", pResponseStr)) - { - if (strcmp(pResponseStr, "Sending capture") == 0) - { - if (ReceiveRTPacket(eType, true, 5000000) > 0) // Wait for QTM file in 5 seconds. - { - if (eType == CRTPacket::PacketQTMFile) - { - if (mpFileBuffer != nullptr) - { - fclose(mpFileBuffer); - return true; - } - strcpy(maErrorStr, "Writing QTM file failed."); - } - else - { - strcpy(maErrorStr, "Wrong packet type received."); - } - } - else - { - sprintf(maErrorStr, "No packet received. %s.", maErrorStr); - } - } - else - { - strcpy(maErrorStr, "GetCaptureQTM failed."); - } - } - else - { - strcpy(maErrorStr, "GetCaptureQTM failed."); - } - } - } - if (mpFileBuffer) - { - fclose(mpFileBuffer); - } - - return false; -} - - -bool CRTProtocol::SendTrig() -{ - char pResponseStr[256]; - - if (SendCommand("Trig", pResponseStr)) - { - if (strcmp(pResponseStr, "Trig ok") == 0) - { - return true; - } - } - if (pResponseStr) - { - sprintf(maErrorStr, "%s.", pResponseStr); - } - else - { - strcpy(maErrorStr, "Trig failed."); - } - return false; -} - - -bool CRTProtocol::SetQTMEvent(const char* pLabel) -{ - char tTemp[100]; - char pResponseStr[256]; - - if (strlen(pLabel) <= 92) - { - sprintf(tTemp, "%s %s", (mnMajorVersion > 1 || mnMinorVersion > 7) ? "SetQTMEvent" : "Event", pLabel); - - if (SendCommand(tTemp, pResponseStr)) - { - if (strcmp(pResponseStr, "Event set") == 0) - { - return true; - } - } - if (pResponseStr) - { - sprintf(maErrorStr, "%s.", pResponseStr); - } - else - { - sprintf(maErrorStr, "%s failed.", (mnMajorVersion > 1 || mnMinorVersion > 7) ? "SetQTMEvent" : "Event"); - } - } - else - { - strcpy(maErrorStr, "Event label too long."); - } - return false; -} - - -bool CRTProtocol::TakeControl(const char* pPassword) -{ - char pResponseStr[256]; - char pCmd[64]; - - strcpy(pCmd, "TakeControl"); - if (pPassword != nullptr) - { - // Add password - if (pPassword[0] != 0) - { - strcat(pCmd, " "); - strcat(pCmd, pPassword); - } - } - if (SendCommand(pCmd, pResponseStr)) - { - if (strcmp("You are now master", pResponseStr) == 0 || - strcmp("You are already master", pResponseStr) == 0) - { - mbIsMaster = true; - return true; - } - } - if (pResponseStr) - { - sprintf(maErrorStr, "%s.", pResponseStr); - } - else - { - strcpy(maErrorStr, "TakeControl failed."); - } - mbIsMaster = false; - return false; -} // TakeControl - - -bool CRTProtocol::ReleaseControl() -{ - char pResponseStr[256]; - - if (SendCommand("ReleaseControl", pResponseStr)) - { - if (strcmp("You are now a regular client", pResponseStr) == 0 || - strcmp("You are already a regular client", pResponseStr) == 0) - { - mbIsMaster = false; - return true; - } - } - if (pResponseStr) - { - sprintf(maErrorStr, "%s.", pResponseStr); - } - else - { - strcpy(maErrorStr, "ReleaseControl failed."); - } - return false; -} // ReleaseControl - - -bool CRTProtocol::IsControlling() -{ - return mbIsMaster; -} - - -bool CRTProtocol::NewMeasurement() -{ - char pResponseStr[256]; - - if (SendCommand("New", pResponseStr)) - { - if (strcmp(pResponseStr, "Creating new connection") == 0 || - strcmp(pResponseStr, "Already connected") == 0) - { - return true; - } - } - if (pResponseStr) - { - sprintf(maErrorStr, "%s.", pResponseStr); - } - else - { - strcpy(maErrorStr, "New failed."); - } - return false; -} - -bool CRTProtocol::CloseMeasurement() -{ - char pResponseStr[256]; - - if (SendCommand("Close", pResponseStr)) - { - if (strcmp(pResponseStr, "Closing connection") == 0 || - strcmp(pResponseStr, "File closed") == 0 || - strcmp(pResponseStr, "Closing file") == 0 || - strcmp(pResponseStr, "No connection to close") == 0) - { - return true; - } - } - if (pResponseStr) - { - sprintf(maErrorStr, "%s.", pResponseStr); - } - else - { - strcpy(maErrorStr, "Close failed."); - } - return false; -} - - -bool CRTProtocol::StartCapture() -{ - char pResponseStr[256]; - - if (SendCommand("Start", pResponseStr)) - { - if (strcmp(pResponseStr, "Starting measurement") == 0) - { - return true; - } - } - if (pResponseStr) - { - sprintf(maErrorStr, "%s.", pResponseStr); - } - else - { - strcpy(maErrorStr, "Start failed."); - } - return false; -} - - -bool CRTProtocol::StartRTOnFile() -{ - char pResponseStr[256]; - - if (SendCommand("Start rtfromfile", pResponseStr)) - { - if (strcmp(pResponseStr, "Starting RT from file") == 0) - { - return true; - } - } - if (pResponseStr) - { - if (strcmp(pResponseStr, "RT from file already running") == 0) - { - return true; - } - sprintf(maErrorStr, "%s.", pResponseStr); - } - else - { - strcpy(maErrorStr, "Starting RT from file failed."); - } - return false; -} - - -bool CRTProtocol::StopCapture() -{ - char pResponseStr[256]; - - if (SendCommand("Stop", pResponseStr)) - { - if (strcmp(pResponseStr, "Stopping measurement") == 0) - { - return true; - } - } - if (pResponseStr) - { - sprintf(maErrorStr, "%s.", pResponseStr); - } - else - { - strcpy(maErrorStr, "Stop failed."); - } - return false; -} - - -bool CRTProtocol::Calibrate(const bool refine, SCalibration &calibrationResult, int timeout) -{ - char pResponseStr[256]; - - if (SendCommand(refine ? "calibrate refine" : "calibrate", pResponseStr)) - { - if (strcmp(pResponseStr, "Starting calibration") == 0) - { - if (ReceiveCalibrationSettings(timeout)) - { - GetCalibrationSettings(calibrationResult); - return true; - } - else - { - return false; - } - } - } - if (pResponseStr) - { - sprintf(maErrorStr, "%s.", pResponseStr); - } - else - { - strcpy(maErrorStr, "Calibrate failed."); - } - return false; -} - - -bool CRTProtocol::LoadCapture(const char* pFileName) -{ - char tTemp[100]; - char pResponseStr[256]; - - if (strlen(pFileName) <= 94) - { - sprintf(tTemp, "Load %s", pFileName); - - if (SendCommand(tTemp, pResponseStr, 20000000)) // Set timeout to 20 s for Load command. - { - if (strcmp(pResponseStr, "Measurement loaded") == 0) - { - return true; - } - } - if (strlen(pResponseStr) > 0) - { - sprintf(maErrorStr, "%s.", pResponseStr); - } - else - { - strcpy(maErrorStr, "Load failed."); - } - } - else - { - strcpy(maErrorStr, "File name too long."); - } - return false; -} - - -bool CRTProtocol::SaveCapture(const char* pFileName, bool bOverwrite, char* pNewFileName, int nSizeOfNewFileName) -{ - char tTemp[100]; - char pResponseStr[256]; - char tNewFileNameTmp[300]; - - tNewFileNameTmp[0] = 0; - - if (strlen(pFileName) <= 94) - { - sprintf(tTemp, "Save %s%s", pFileName, bOverwrite ? " Overwrite" : ""); - - if (SendCommand(tTemp, pResponseStr)) - { - if (strcmp(pResponseStr, "Measurement saved") == 0) - { - if (pNewFileName && nSizeOfNewFileName > 0) - { - pNewFileName[0] = 0; - } - return true; - } - if (sscanf(pResponseStr, "Measurement saved as '%[^']'", tNewFileNameTmp) == 1) - { - if (pNewFileName) - { - strcpy(pNewFileName, tNewFileNameTmp); - } - return true; - } - } - if (pResponseStr) - { - sprintf(maErrorStr, "%s.", pResponseStr); - } - else - { - strcpy(maErrorStr, "Save failed."); - } - } - else - { - strcpy(maErrorStr, "File name too long."); - } - return false; -} - - -bool CRTProtocol::LoadProject(const char* pFileName) -{ - char tTemp[100]; - char pResponseStr[256]; - - if (strlen(pFileName) <= 94) - { - sprintf(tTemp, "LoadProject %s", pFileName); - - if (SendCommand(tTemp, pResponseStr, 20000000)) // Timeout 20 s - { - if (strcmp(pResponseStr, "Project loaded") == 0) - { - return true; - } - } - if (pResponseStr) - { - sprintf(maErrorStr, "%s.", pResponseStr); - } - else - { - strcpy(maErrorStr, "Load project failed."); - } - } - else - { - strcpy(maErrorStr, "File name too long."); - } - return false; -} - - -bool CRTProtocol::Reprocess() -{ - char pResponseStr[256]; - - if (SendCommand("Reprocess", pResponseStr)) - { - if (strcmp(pResponseStr, "Reprocessing file") == 0) - { - return true; - } - } - if (pResponseStr) - { - sprintf(maErrorStr, "%s.", pResponseStr); - } - else - { - strcpy(maErrorStr, "Reprocess failed."); - } - return false; -} - - -bool CRTProtocol::GetEventString(CRTPacket::EEvent eEvent, char* pStr) -{ - switch (eEvent) - { - case CRTPacket::EventConnected: strcpy(pStr, "Connected"); - break; - case CRTPacket::EventConnectionClosed: strcpy(pStr, "Connection Closed"); - break; - case CRTPacket::EventCaptureStarted: strcpy(pStr, "Capture Started"); - break; - case CRTPacket::EventCaptureStopped: strcpy(pStr, "Capture Stopped"); - break; - case CRTPacket::EventCaptureFetchingFinished: strcpy(pStr, "Fetching Finished"); - break; - case CRTPacket::EventCalibrationStarted: strcpy(pStr, "Calibration Started"); - break; - case CRTPacket::EventCalibrationStopped: strcpy(pStr, "Calibration Finished"); - break; - case CRTPacket::EventRTfromFileStarted: strcpy(pStr, "RT From File Started"); - break; - case CRTPacket::EventRTfromFileStopped: strcpy(pStr, "RT From File Stopped"); - break; - case CRTPacket::EventWaitingForTrigger: strcpy(pStr, "Waiting For Trigger"); - break; - case CRTPacket::EventCameraSettingsChanged: strcpy(pStr, "Camera Settings Changed"); - break; - case CRTPacket::EventQTMShuttingDown: strcpy(pStr, "QTM Shutting Down"); - break; - case CRTPacket::EventCaptureSaved: strcpy(pStr, "Capture Saved"); - break; - case CRTPacket::EventReprocessingStarted: strcpy(pStr, "Reprocessing Started"); - break; - case CRTPacket::EventReprocessingStopped: strcpy(pStr, "Reprocessing Stopped"); - break; - case CRTPacket::EventTrigger: strcpy(pStr, "Trigger"); - break; - default: - return false; - } - return true; -} - - -bool CRTProtocol::ConvertRateString(const char* pRate, EStreamRate &eRate, unsigned int &nRateArg) -{ - std::string rateString; - - rateString.assign(pRate); - std::transform(rateString.begin(), rateString.end(), rateString.begin(), ::tolower); - - eRate = RateNone; - - if (rateString.compare(0, 9, "allframes", 9) == 0) - { - eRate = RateAllFrames; - } - else if (rateString.compare(0, 10, "frequency:") == 0) - { - nRateArg = atoi(rateString.substr(10).c_str()); - if (nRateArg > 0) - { - eRate = RateFrequency; - } - } - else if (rateString.compare(0, 17, "frequencydivisor:") == 0) - { - nRateArg = atoi(rateString.substr(17).c_str()); - if (nRateArg > 0) - { - eRate = RateFrequencyDivisor; - } - } - - return eRate != RateNone; -} - -std::vector> CRTProtocol::GetComponents(const std::string componentsString) -{ - std::vector> components; // Vector containing pair (component, option string). - auto result = std::back_inserter(components); - std::istringstream iss(componentsString); - std::string item; - while (std::getline(iss, item, ' ')) - { - unsigned int component; - std::string option; - if (GetComponent(item, component, option)) - { - *result++ = std::make_pair(component, option); - } - else - { - // Parsing failed. Unrecognized component. - components.clear(); - break; - } - } - - return components; -} - -bool CRTProtocol::GetComponent(std::string componentStr, unsigned int &component, std::string &option) -{ - // Make string lower case. - std::transform(componentStr.begin(), componentStr.end(), componentStr.begin(), ::tolower); - option = ""; - - if (componentStr == "2d") - { - component = CRTProtocol::cComponent2d; - return true; - } - if (componentStr == "2dlin") - { - component = CRTProtocol::cComponent2dLin; - return true; - } - if (componentStr == "3d") - { - component = CRTProtocol::cComponent3d; - return true; - } - if (componentStr == "3dres") - { - component = CRTProtocol::cComponent3dRes; - return true; - } - if (componentStr == "3dnolabels") - { - component = CRTProtocol::cComponent3dNoLabels; - return true; - } - if (componentStr == "3dnolabelsres") - { - component = CRTProtocol::cComponent3dNoLabelsRes; - return true; - } - if (componentStr == "analogsingle") - { - component = CRTProtocol::cComponentAnalogSingle; - return true; - } - if (componentStr.find("analogsingle:") != std::string::npos) - { - option = componentStr.substr(strlen("analogsingle:")); - component = CRTProtocol::cComponentAnalogSingle; - return true; - } - if (componentStr == "analog") - { - component = CRTProtocol::cComponentAnalog; - return true; - } - if (componentStr.find("analog:") != std::string::npos) - { - option = componentStr.substr(strlen("analog:")); - component = CRTProtocol::cComponentAnalog; - return true; - } - if (componentStr == "force") - { - component = CRTProtocol::cComponentForce; - return true; - } - if (componentStr == "forcesingle") - { - component = CRTProtocol::cComponentForceSingle; - return true; - } - if (componentStr == "6d") - { - component = CRTProtocol::cComponent6d; - return true; - } - if (componentStr == "6dres") - { - component = CRTProtocol::cComponent6dRes; - return true; - } - if (componentStr == "6deuler") - { - component = CRTProtocol::cComponent6dEuler; - return true; - } - if (componentStr == "6deulerres") - { - component = CRTProtocol::cComponent6dEulerRes; - return true; - } - if (componentStr == "image") - { - component = CRTProtocol::cComponentImage; - return true; - } - if (componentStr == "gazevector") - { - component = CRTProtocol::cComponentGazeVector; - return true; - } - if (componentStr == "timecode") - { - component = CRTProtocol::cComponentTimecode; - return true; - } - if (componentStr == "skeleton") - { - component = CRTProtocol::cComponentSkeleton; - return true; - } - if (componentStr == "skeleton:global") - { - option = "global"; - component = CRTProtocol::cComponentSkeleton; - return true; - } - return false; -} - - -unsigned int CRTProtocol::ConvertComponentString(const char* componentsString) -{ - auto components = GetComponents(std::string(componentsString)); - - unsigned int componentTypes = 0; - - for (auto const& component : components) - { - componentTypes += component.first; - } - - return componentTypes; -} - - -bool CRTProtocol::GetComponentString(char* pComponentStr, unsigned int nComponentType, const SComponentOptions& options) -{ - pComponentStr[0] = 0; - - if (nComponentType & cComponent2d) - { - strcat(pComponentStr, "2D "); - } - if (nComponentType & cComponent2dLin) - { - strcat(pComponentStr, "2DLin "); - } - if (nComponentType & cComponent3d) - { - strcat(pComponentStr, "3D "); - } - if (nComponentType & cComponent3dRes) - { - strcat(pComponentStr, "3DRes "); - } - if (nComponentType & cComponent3dNoLabels) - { - strcat(pComponentStr, "3DNoLabels "); - } - if (nComponentType & cComponent3dNoLabelsRes) - { - strcat(pComponentStr, "3DNoLabelsRes "); - } - if (nComponentType & cComponent6d) - { - strcat(pComponentStr, "6D "); - } - if (nComponentType & cComponent6dRes) - { - strcat(pComponentStr, "6DRes "); - } - if (nComponentType & cComponent6dEuler) - { - strcat(pComponentStr, "6DEuler "); - } - if (nComponentType & cComponent6dEulerRes) - { - strcat(pComponentStr, "6DEulerRes "); - } - if (nComponentType & cComponentAnalog) - { - strcat(pComponentStr, "Analog"); - - if (options.mAnalogChannels != nullptr) - { - strcat(pComponentStr, ":"); - strcat(pComponentStr, options.mAnalogChannels); - } - - strcat(pComponentStr, " "); - } - if (nComponentType & cComponentAnalogSingle) - { - strcat(pComponentStr, "AnalogSingle"); - - if (options.mAnalogChannels != nullptr) - { - strcat(pComponentStr, ":"); - strcat(pComponentStr, options.mAnalogChannels); - } - - strcat(pComponentStr, " "); - } - if (nComponentType & cComponentForce) - { - strcat(pComponentStr, "Force "); - } - if (nComponentType & cComponentForceSingle) - { - strcat(pComponentStr, "ForceSingle "); - } - if (nComponentType & cComponentGazeVector) - { - strcat(pComponentStr, "GazeVector "); - } - if (nComponentType & cComponentImage) - { - strcat(pComponentStr, "Image "); - } - if (nComponentType & cComponentTimecode) - { - strcat(pComponentStr, "Timecode "); - } - if (nComponentType & cComponentSkeleton) - { - strcat(pComponentStr, "Skeleton"); - - if (options.mSkeletonGlobalData) - { - strcat(pComponentStr, ":global"); - } - - strcat(pComponentStr, " "); - } - - return (pComponentStr[0] != 0); -} - - -int CRTProtocol::ReceiveRTPacket(CRTPacket::EPacketType &eType, bool bSkipEvents, int nTimeout) -{ - int nRecved = 0; - unsigned int nRecvedTotal = 0; - unsigned int nFrameSize; - - eType = CRTPacket::PacketNone; - - do - { - nRecved = 0; - nRecvedTotal = 0; - - nRecved = mpoNetwork->Receive(maDataBuff, mDataBuffSize, true, nTimeout); - if (nRecved == 0) - { - // Receive timeout. - strcpy(maErrorStr, "Data receive timeout."); - return 0; - } - if (nRecved < (int)(sizeof(int) * 2)) - { - // QTM header not received. - strcpy(maErrorStr, "Couldn't read header bytes."); - return -1; - } - if (nRecved <= -1) - { - strcpy(maErrorStr, "Socket Error."); - return -1; - } - nRecvedTotal += nRecved; - - bool bBigEndian = (mbBigEndian || (mnMajorVersion == 1 && mnMinorVersion == 0)); - nFrameSize = mpoRTPacket->GetSize(maDataBuff, bBigEndian); - eType = mpoRTPacket->GetType(maDataBuff, bBigEndian); - - unsigned int nReadSize; - - if (eType == CRTPacket::PacketC3DFile || eType == CRTPacket::PacketQTMFile) - { - if (mpFileBuffer != nullptr) - { - rewind(mpFileBuffer); // Start from the beginning - if (fwrite(maDataBuff + sizeof(int) * 2, 1, nRecvedTotal - sizeof(int) * 2, mpFileBuffer) != - nRecvedTotal - sizeof(int) * 2) - { - strcpy(maErrorStr, "Failed to write file to disk."); - fclose(mpFileBuffer); - mpFileBuffer = nullptr; - return -1; - } - // Receive more data until we have read the whole packet - while (nRecvedTotal < nFrameSize) - { - nReadSize = nFrameSize - nRecvedTotal; - if (nFrameSize > mDataBuffSize) - { - nReadSize = mDataBuffSize; - } - // As long as we haven't received enough data, wait for more - nRecved = mpoNetwork->Receive(&(maDataBuff[sizeof(int) * 2]), nReadSize, false, nTimeout); - if (nRecved <= 0) - { - strcpy(maErrorStr, "Socket Error."); - fclose(mpFileBuffer); - mpFileBuffer = nullptr; - return -1; - } - if (fwrite(maDataBuff + sizeof(int) * 2, 1, nRecved, mpFileBuffer) != (size_t)nRecved) - { - strcpy(maErrorStr, "Failed to write file to disk."); - fclose(mpFileBuffer); - mpFileBuffer = nullptr; - return -1; - } - nRecvedTotal += nRecved; - } - } - else - { - strcpy(maErrorStr, "Receive file buffer not opened."); - if (mpFileBuffer) - { - fclose(mpFileBuffer); - } - mpFileBuffer = nullptr; - return -1; - } - } - else - { - if (nFrameSize > mDataBuffSize) - { - char* buf = new char[nFrameSize]; - memcpy(buf, maDataBuff, mDataBuffSize); - delete maDataBuff; - maDataBuff = buf; - mDataBuffSize = nFrameSize; - } - - // Receive more data until we have read the whole packet - while (nRecvedTotal < nFrameSize) - { - // As long as we haven't received enough data, wait for more - nRecved = mpoNetwork->Receive(&(maDataBuff[nRecvedTotal]), nFrameSize - nRecvedTotal, false, nTimeout); - if (nRecved <= 0) - { - strcpy(maErrorStr, "Socket Error."); - return -1; - } - nRecvedTotal += nRecved; - } - } - - mpoRTPacket->SetData(maDataBuff); - - if (mpoRTPacket->GetEvent(meLastEvent)) // Update last event if there is an event - { - if (meLastEvent != CRTPacket::EventCameraSettingsChanged) - { - meState = meLastEvent; - } - } - } while (bSkipEvents && eType == CRTPacket::PacketEvent); - - if (nRecvedTotal == nFrameSize) - { - return nRecvedTotal; - } - strcpy(maErrorStr, "Packet truncated."); - - return -1; -} // ReceiveRTPacket - - -CRTPacket* CRTProtocol::GetRTPacket() -{ - return mpoRTPacket; -} - - -bool CRTProtocol::ReadXmlBool(CMarkup* xml, const std::string& element, bool& value) const -{ - if (!xml->FindChildElem(element.c_str())) - { - return false; - } - - auto str = xml->GetChildData(); - RemoveInvalidChars(str); - ToLower(str); - - if (str == "true") - { - value = true; - } - else if (str == "false") - { - value = false; - } - else - { - // Don't change value, just report error. - return false; - } - - return true; -} - -bool CRTProtocol::ReadCameraSystemSettings() -{ - CRTPacket::EPacketType eType; - CMarkup oXML; - std::string tStr; - - msGeneralSettings.vsCameras.clear(); - - if (!SendCommand("GetParameters General")) - { - strcpy(maErrorStr, "GetParameters General failed"); - return false; - } - - auto received = ReceiveRTPacket(eType, true); - if (received <= 0) - { - if (received == 0) - { - // Receive timeout. - strcat(maErrorStr, " Expected XML packet."); - } - return false; - } - - if (eType != CRTPacket::PacketXML) - { - if (eType == CRTPacket::PacketError) - { - sprintf(maErrorStr, "%s.", mpoRTPacket->GetErrorString()); - } - else - { - sprintf(maErrorStr, "GetParameters General returned wrong packet type. Got type %d expected type 2.", eType); - } - return false; - } - - oXML.SetDoc(mpoRTPacket->GetXMLString()); - - // ==================== General ==================== - if (!oXML.FindChildElem("General")) - { - return false; - } - oXML.IntoElem(); - - if (!oXML.FindChildElem("Frequency")) - { - return false; - } - msGeneralSettings.nCaptureFrequency = atoi(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Capture_Time")) - { - return false; - } - msGeneralSettings.fCaptureTime = (float)atof(oXML.GetChildData().c_str()); - - // Refactored variant of all this copy/paste code. TODO: Refactor everything else. - if (!ReadXmlBool(&oXML, "Start_On_External_Trigger", msGeneralSettings.bStartOnExternalTrigger)) - { - return false; - } - if (mnMajorVersion > 1 || mnMinorVersion > 14) - { - if (!ReadXmlBool(&oXML, "Start_On_Trigger_NO", msGeneralSettings.bStartOnTrigNO)) - { - return false; - } - if (!ReadXmlBool(&oXML, "Start_On_Trigger_NC", msGeneralSettings.bStartOnTrigNC)) - { - return false; - } - if (!ReadXmlBool(&oXML, "Start_On_Trigger_Software", msGeneralSettings.bStartOnTrigSoftware)) - { - return false; - } - } - - // ==================== External time base ==================== - if (!oXML.FindChildElem("External_Time_Base")) - { - return false; - } - oXML.IntoElem(); - - if (!oXML.FindChildElem("Enabled")) - { - return false; - } - tStr = oXML.GetChildData(); - std::transform(tStr.begin(), tStr.end(), tStr.begin(), ::tolower); - msGeneralSettings.sExternalTimebase.bEnabled = (tStr == "true"); - - if (!oXML.FindChildElem("Signal_Source")) - { - return false; - } - tStr = oXML.GetChildData(); - std::transform(tStr.begin(), tStr.end(), tStr.begin(), ::tolower); - if (tStr == "control port") - { - msGeneralSettings.sExternalTimebase.eSignalSource = SourceControlPort; - } - else if (tStr == "ir receiver") - { - msGeneralSettings.sExternalTimebase.eSignalSource = SourceIRReceiver; - } - else if (tStr == "smpte") - { - msGeneralSettings.sExternalTimebase.eSignalSource = SourceSMPTE; - } - else if (tStr == "irig") - { - msGeneralSettings.sExternalTimebase.eSignalSource = SourceIRIG; - } - else if (tStr == "video sync") - { - msGeneralSettings.sExternalTimebase.eSignalSource = SourceVideoSync; - } - else - { - return false; - } - - if (!oXML.FindChildElem("Signal_Mode")) - { - return false; - } - tStr = oXML.GetChildData(); - std::transform(tStr.begin(), tStr.end(), tStr.begin(), ::tolower); - if (tStr == "periodic") - { - msGeneralSettings.sExternalTimebase.bSignalModePeriodic = true; - } - else if (tStr == "non-periodic") - { - msGeneralSettings.sExternalTimebase.bSignalModePeriodic = false; - } - else - { - return false; - } - - if (!oXML.FindChildElem("Frequency_Multiplier")) - { - return false; - } - unsigned int nMultiplier; - tStr = oXML.GetChildData(); - if (sscanf(tStr.c_str(), "%u", &nMultiplier) == 1) - { - msGeneralSettings.sExternalTimebase.nFreqMultiplier = nMultiplier; - } - else - { - return false; - } - - if (!oXML.FindChildElem("Frequency_Divisor")) - { - return false; - } - unsigned int nDivisor; - tStr = oXML.GetChildData(); - if (sscanf(tStr.c_str(), "%u", &nDivisor) == 1) - { - msGeneralSettings.sExternalTimebase.nFreqDivisor = nDivisor; - } - else - { - return false; - } - - if (!oXML.FindChildElem("Frequency_Tolerance")) - { - return false; - } - unsigned int nTolerance; - tStr = oXML.GetChildData(); - if (sscanf(tStr.c_str(), "%u", &nTolerance) == 1) - { - msGeneralSettings.sExternalTimebase.nFreqTolerance = nTolerance; - } - else - { - return false; - } - - if (!oXML.FindChildElem("Nominal_Frequency")) - { - return false; - } - tStr = oXML.GetChildData(); - std::transform(tStr.begin(), tStr.end(), tStr.begin(), ::tolower); - - if (tStr == "none") - { - msGeneralSettings.sExternalTimebase.fNominalFrequency = -1; // -1 = disabled - } - else - { - float fFrequency; - if (sscanf(tStr.c_str(), "%f", &fFrequency) == 1) - { - msGeneralSettings.sExternalTimebase.fNominalFrequency = fFrequency; - } - else - { - return false; - } - } - - if (!oXML.FindChildElem("Signal_Edge")) - { - return false; - } - tStr = oXML.GetChildData(); - std::transform(tStr.begin(), tStr.end(), tStr.begin(), ::tolower); - if (tStr == "negative") - { - msGeneralSettings.sExternalTimebase.bNegativeEdge = true; - } - else if (tStr == "positive") - { - msGeneralSettings.sExternalTimebase.bNegativeEdge = false; - } - else - { - return false; - } - - if (!oXML.FindChildElem("Signal_Shutter_Delay")) - { - return false; - } - unsigned int nDelay; - tStr = oXML.GetChildData(); - if (sscanf(tStr.c_str(), "%u", &nDelay) == 1) - { - msGeneralSettings.sExternalTimebase.nSignalShutterDelay = nDelay; - } - else - { - return false; - } - - if (!oXML.FindChildElem("Non_Periodic_Timeout")) - { - return false; - } - float fTimeout; - tStr = oXML.GetChildData(); - if (sscanf(tStr.c_str(), "%f", &fTimeout) == 1) - { - msGeneralSettings.sExternalTimebase.fNonPeriodicTimeout = fTimeout; - } - else - { - return false; - } - - oXML.OutOfElem(); // External_Time_Base - - const char* processings[3] = { "Processing_Actions", "RealTime_Processing_Actions", "Reprocessing_Actions" }; - EProcessingActions* processingActions[3] = - { - &msGeneralSettings.eProcessingActions, - &msGeneralSettings.eRtProcessingActions, - &msGeneralSettings.eReprocessingActions - }; - auto actionsCount = (mnMajorVersion > 1 || mnMinorVersion > 13) ? 3 : 1; - for (auto i = 0; i < actionsCount; i++) - { - // ==================== Processing actions ==================== - if (!oXML.FindChildElem(processings[i])) - { - return false; - } - oXML.IntoElem(); - - *processingActions[i] = ProcessingNone; - - if (mnMajorVersion > 1 || mnMinorVersion > 13) - { - if (!oXML.FindChildElem("PreProcessing2D")) - { - return false; - } - if (CompareNoCase(oXML.GetChildData(), "true")) - { - *processingActions[i] = (EProcessingActions)(*processingActions[i] + ProcessingPreProcess2D); - } - } - - if (!oXML.FindChildElem("Tracking")) - { - return false; - } - tStr = oXML.GetChildData(); - std::transform(tStr.begin(), tStr.end(), tStr.begin(), ::tolower); - if (tStr == "3d") - { - *processingActions[i] = (EProcessingActions)(*processingActions[i] + ProcessingTracking3D); - } - else if (tStr == "2d" && i != 1) // i != 1 => Not RtProcessingSettings - { - *processingActions[i] = (EProcessingActions)(*processingActions[i] + ProcessingTracking2D); - } - - if (i != 1) //Not RtProcessingSettings - { - if (!oXML.FindChildElem("TwinSystemMerge")) - { - return false; - } - if (CompareNoCase(oXML.GetChildData(), "true")) - { - *processingActions[i] = (EProcessingActions)(*processingActions[i] + ProcessingTwinSystemMerge); - } - - if (!oXML.FindChildElem("SplineFill")) - { - return false; - } - if (CompareNoCase(oXML.GetChildData(), "true")) - { - *processingActions[i] = (EProcessingActions)(*processingActions[i] + ProcessingSplineFill); - } - } - - if (!oXML.FindChildElem("AIM")) - { - return false; - } - if (CompareNoCase(oXML.GetChildData(), "true")) - { - *processingActions[i] = (EProcessingActions)(*processingActions[i] + ProcessingAIM); - } - - if (!oXML.FindChildElem("Track6DOF")) - { - return false; - } - if (CompareNoCase(oXML.GetChildData(), "true")) - { - *processingActions[i] = (EProcessingActions)(*processingActions[i] + Processing6DOFTracking); - } - - if (!oXML.FindChildElem("ForceData")) - { - return false; - } - if (CompareNoCase(oXML.GetChildData(), "true")) - { - *processingActions[i] = (EProcessingActions)(*processingActions[i] + ProcessingForceData); - } - - if (mnMajorVersion > 1 || mnMinorVersion > 11) - { - if (!oXML.FindChildElem("GazeVector")) - { - return false; - } - if (CompareNoCase(oXML.GetChildData(), "true")) - { - *processingActions[i] = (EProcessingActions)(*processingActions[i] + ProcessingGazeVector); - } - } - - if (i != 1) //Not RtProcessingSettings - { - if (!oXML.FindChildElem("ExportTSV")) - { - return false; - } - if (CompareNoCase(oXML.GetChildData(), "true")) - { - *processingActions[i] = (EProcessingActions)(*processingActions[i] + ProcessingExportTSV); - } - - if (!oXML.FindChildElem("ExportC3D")) - { - return false; - } - if (CompareNoCase(oXML.GetChildData(), "true")) - { - *processingActions[i] = (EProcessingActions)(*processingActions[i] + ProcessingExportC3D); - } - - if (!oXML.FindChildElem("ExportMatlabFile")) - { - return false; - } - if (CompareNoCase(oXML.GetChildData(), "true")) - { - *processingActions[i] = (EProcessingActions)(*processingActions[i] + ProcessingExportMatlabFile); - } - - if (mnMajorVersion > 1 || mnMinorVersion > 11) - { - if (!oXML.FindChildElem("ExportAviFile")) - { - return false; - } - if (CompareNoCase(oXML.GetChildData(), "true")) - { - *processingActions[i] = (EProcessingActions)(*processingActions[i] + ProcessingExportAviFile); - } - } - } - oXML.OutOfElem(); // Processing_Actions - } - - SSettingsGeneralCamera sCameraSettings; - - while (oXML.FindChildElem("Camera")) - { - oXML.IntoElem(); - - if (!oXML.FindChildElem("ID")) - { - return false; - } - sCameraSettings.nID = atoi(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Model")) - { - return false; - } - tStr = oXML.GetChildData(); - std::transform(tStr.begin(), tStr.end(), tStr.begin(), ::tolower); - - if (tStr == "macreflex") - { - sCameraSettings.eModel = ModelMacReflex; - } - else if (tStr == "proreflex 120") - { - sCameraSettings.eModel = ModelProReflex120; - } - else if (tStr == "proreflex 240") - { - sCameraSettings.eModel = ModelProReflex240; - } - else if (tStr == "proreflex 500") - { - sCameraSettings.eModel = ModelProReflex500; - } - else if (tStr == "proreflex 1000") - { - sCameraSettings.eModel = ModelProReflex1000; - } - else if (tStr == "oqus 100") - { - sCameraSettings.eModel = ModelOqus100; - } - else if (tStr == "oqus 200" || tStr == "oqus 200 c") - { - sCameraSettings.eModel = ModelOqus200C; - } - else if (tStr == "oqus 300") - { - sCameraSettings.eModel = ModelOqus300; - } - else if (tStr == "oqus 300 plus") - { - sCameraSettings.eModel = ModelOqus300Plus; - } - else if (tStr == "oqus 400") - { - sCameraSettings.eModel = ModelOqus400; - } - else if (tStr == "oqus 500") - { - sCameraSettings.eModel = ModelOqus500; - } - else if (tStr == "oqus 500 plus") - { - sCameraSettings.eModel = ModelOqus500Plus; - } - else if (tStr == "oqus 700") - { - sCameraSettings.eModel = ModelOqus700; - } - else if (tStr == "oqus 700 plus") - { - sCameraSettings.eModel = ModelOqus700Plus; - } - else if (tStr == "oqus 600 plus") - { - sCameraSettings.eModel = ModelOqus600Plus; - } - else if (tStr == "miqus m1") - { - sCameraSettings.eModel = ModelMiqusM1; - } - else if (tStr == "miqus m3") - { - sCameraSettings.eModel = ModelMiqusM3; - } - else if (tStr == "miqus m5") - { - sCameraSettings.eModel = ModelMiqusM5; - } - else if (tStr == "miqus sync unit") - { - sCameraSettings.eModel = ModelMiqusSyncUnit; - } - else if (tStr == "miqus video") - { - sCameraSettings.eModel = ModelMiqusVideo; - } - else if (tStr == "miqus video color") - { - sCameraSettings.eModel = ModelMiqusVideoColor; - } - else - { - return false; - } - - // Only available from protocol version 1.10 and later. - if (oXML.FindChildElem("Underwater")) - { - tStr = oXML.GetChildData(); - std::transform(tStr.begin(), tStr.end(), tStr.begin(), ::tolower); - sCameraSettings.bUnderwater = (tStr == "true"); - } - - if (oXML.FindChildElem("Supports_HW_Sync")) - { - tStr = oXML.GetChildData(); - std::transform(tStr.begin(), tStr.end(), tStr.begin(), ::tolower); - sCameraSettings.bSupportsHwSync = (tStr == "true"); - } - - if (!oXML.FindChildElem("Serial")) - { - return false; - } - sCameraSettings.nSerial = atoi(oXML.GetChildData().c_str()); - - // ==================== Camera Mode ==================== - if (!oXML.FindChildElem("Mode")) - { - return false; - } - tStr = oXML.GetChildData(); - std::transform(tStr.begin(), tStr.end(), tStr.begin(), ::tolower); - if (tStr == "marker") - { - sCameraSettings.eMode = ModeMarker; - } - else if (tStr == "marker intensity") - { - sCameraSettings.eMode = ModeMarkerIntensity; - } - else if (tStr == "video") - { - sCameraSettings.eMode = ModeVideo; - } - else - { - return false; - } - - if (mnMajorVersion > 1 || mnMinorVersion > 11) - { - // ==================== Video frequency ==================== - if (!oXML.FindChildElem("Video_Frequency")) - { - return false; - } - sCameraSettings.nVideoFrequency = atoi(oXML.GetChildData().c_str()); - } - - // ==================== Video Resolution ==================== - if (oXML.FindChildElem("Video_Resolution")) - { - tStr = oXML.GetChildData(); - std::transform(tStr.begin(), tStr.end(), tStr.begin(), ::tolower); - if (tStr == "1080p") - { - sCameraSettings.eVideoResolution = VideoResolution1080p; - } - else if (tStr == "720p") - { - sCameraSettings.eVideoResolution = VideoResolution720p; - } - else if (tStr == "540p") - { - sCameraSettings.eVideoResolution = VideoResolution540p; - } - else if (tStr == "480p") - { - sCameraSettings.eVideoResolution = VideoResolution480p; - } - } - else - { - sCameraSettings.eVideoResolution = VideoResolutionNone; - } - - // ==================== Video AspectRatio ==================== - if (oXML.FindChildElem("Video_Aspect_Ratio")) - { - tStr = oXML.GetChildData(); - std::transform(tStr.begin(), tStr.end(), tStr.begin(), ::tolower); - if (tStr == "16x9") - { - sCameraSettings.eVideoAspectRatio = VideoAspectRatio16x9; - } - else if (tStr == "4x3") - { - sCameraSettings.eVideoAspectRatio = VideoAspectRatio4x3; - } - else if (tStr == "1x1") - { - sCameraSettings.eVideoAspectRatio = VideoAspectRatio1x1; - } - } - else - { - sCameraSettings.eVideoAspectRatio = VideoAspectRatioNone; - } - - // ==================== Video exposure ==================== - if (!oXML.FindChildElem("Video_Exposure")) - { - return false; - } - oXML.IntoElem(); - - if (!oXML.FindChildElem("Current")) - { - return false; - } - sCameraSettings.nVideoExposure = atoi(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Min")) - { - return false; - } - sCameraSettings.nVideoExposureMin = atoi(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Max")) - { - return false; - } - sCameraSettings.nVideoExposureMax = atoi(oXML.GetChildData().c_str()); - oXML.OutOfElem(); // Video_Exposure - - // ==================== Video flash time ==================== - if (!oXML.FindChildElem("Video_Flash_Time")) - { - return false; - } - oXML.IntoElem(); - - if (!oXML.FindChildElem("Current")) - { - return false; - } - sCameraSettings.nVideoFlashTime = atoi(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Min")) - { - return false; - } - sCameraSettings.nVideoFlashTimeMin = atoi(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Max")) - { - return false; - } - sCameraSettings.nVideoFlashTimeMax = atoi(oXML.GetChildData().c_str()); - oXML.OutOfElem(); // Video_Flash_Time - - // ==================== Marker exposure ==================== - if (!oXML.FindChildElem("Marker_Exposure")) - { - return false; - } - oXML.IntoElem(); - - if (!oXML.FindChildElem("Current")) - { - return false; - } - sCameraSettings.nMarkerExposure = atoi(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Min")) - { - return false; - } - sCameraSettings.nMarkerExposureMin = atoi(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Max")) - { - return false; - } - sCameraSettings.nMarkerExposureMax = atoi(oXML.GetChildData().c_str()); - - oXML.OutOfElem(); // Marker_Exposure - - // ==================== Marker threshold ==================== - if (!oXML.FindChildElem("Marker_Threshold")) - { - return false; - } - oXML.IntoElem(); - - if (!oXML.FindChildElem("Current")) - { - return false; - } - sCameraSettings.nMarkerThreshold = atoi(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Min")) - { - return false; - } - sCameraSettings.nMarkerThresholdMin = atoi(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Max")) - { - return false; - } - sCameraSettings.nMarkerThresholdMax = atoi(oXML.GetChildData().c_str()); - - oXML.OutOfElem(); // Marker_Threshold - - // ==================== Position ==================== - if (!oXML.FindChildElem("Position")) - { - return false; - } - oXML.IntoElem(); - - if (!oXML.FindChildElem("X")) - { - return false; - } - sCameraSettings.fPositionX = (float)atoi(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Y")) - { - return false; - } - sCameraSettings.fPositionY = (float)atoi(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Z")) - { - return false; - } - sCameraSettings.fPositionZ = (float)atoi(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Rot_1_1")) - { - return false; - } - sCameraSettings.fPositionRotMatrix[0][0] = (float)atof(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Rot_2_1")) - { - return false; - } - sCameraSettings.fPositionRotMatrix[1][0] = (float)atof(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Rot_3_1")) - { - return false; - } - sCameraSettings.fPositionRotMatrix[2][0] = (float)atof(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Rot_1_2")) - { - return false; - } - sCameraSettings.fPositionRotMatrix[0][1] = (float)atof(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Rot_2_2")) - { - return false; - } - sCameraSettings.fPositionRotMatrix[1][1] = (float)atof(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Rot_3_2")) - { - return false; - } - sCameraSettings.fPositionRotMatrix[2][1] = (float)atof(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Rot_1_3")) - { - return false; - } - sCameraSettings.fPositionRotMatrix[0][2] = (float)atof(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Rot_2_3")) - { - return false; - } - sCameraSettings.fPositionRotMatrix[1][2] = (float)atof(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Rot_3_3")) - { - return false; - } - sCameraSettings.fPositionRotMatrix[2][2] = (float)atof(oXML.GetChildData().c_str()); - - oXML.OutOfElem(); // Position - - - if (!oXML.FindChildElem("Orientation")) - { - return false; - } - sCameraSettings.nOrientation = atoi(oXML.GetChildData().c_str()); - - // ==================== Marker resolution ==================== - if (!oXML.FindChildElem("Marker_Res")) - { - return false; - } - oXML.IntoElem(); - - if (!oXML.FindChildElem("Width")) - { - return false; - } - sCameraSettings.nMarkerResolutionWidth = atoi(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Height")) - { - return false; - } - sCameraSettings.nMarkerResolutionHeight = atoi(oXML.GetChildData().c_str()); - - oXML.OutOfElem(); // Marker_Res - - // ==================== Video resolution ==================== - if (!oXML.FindChildElem("Video_Res")) - { - return false; - } - oXML.IntoElem(); - - if (!oXML.FindChildElem("Width")) - { - return false; - } - sCameraSettings.nVideoResolutionWidth = atoi(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Height")) - { - return false; - } - sCameraSettings.nVideoResolutionHeight = atoi(oXML.GetChildData().c_str()); - - oXML.OutOfElem(); // Video_Res - - // ==================== Marker FOV ==================== - if (!oXML.FindChildElem("Marker_FOV")) - { - return false; - } - oXML.IntoElem(); - - if (!oXML.FindChildElem("Left")) - { - return false; - } - sCameraSettings.nMarkerFOVLeft = atoi(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Top")) - { - return false; - } - sCameraSettings.nMarkerFOVTop = atoi(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Right")) - { - return false; - } - sCameraSettings.nMarkerFOVRight = atoi(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Bottom")) - { - return false; - } - sCameraSettings.nMarkerFOVBottom = atoi(oXML.GetChildData().c_str()); - - oXML.OutOfElem(); // Marker_FOV - - // ==================== Video FOV ==================== - if (!oXML.FindChildElem("Video_FOV")) - { - return false; - } - oXML.IntoElem(); - - if (!oXML.FindChildElem("Left")) - { - return false; - } - sCameraSettings.nVideoFOVLeft = atoi(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Top")) - { - return false; - } - sCameraSettings.nVideoFOVTop = atoi(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Right")) - { - return false; - } - sCameraSettings.nVideoFOVRight = atoi(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Bottom")) - { - return false; - } - sCameraSettings.nVideoFOVBottom = atoi(oXML.GetChildData().c_str()); - - oXML.OutOfElem(); // Video_FOV - - // ==================== Sync out ==================== - // Only available from protocol version 1.10 and later. - for (int port = 0; port < 3; port++) - { - char syncOutStr[16]; - sprintf(syncOutStr, "Sync_Out%s", port == 0 ? "" : (port == 1 ? "2" : "_MT")); - if (oXML.FindChildElem(syncOutStr)) - { - oXML.IntoElem(); - - if (port < 2) - { - if (!oXML.FindChildElem("Mode")) - { - return false; - } - tStr = oXML.GetChildData(); - std::transform(tStr.begin(), tStr.end(), tStr.begin(), ::tolower); - if (tStr == "shutter out") - { - sCameraSettings.eSyncOutMode[port] = ModeShutterOut; - } - else if (tStr == "multiplier") - { - sCameraSettings.eSyncOutMode[port] = ModeMultiplier; - } - else if (tStr == "divisor") - { - sCameraSettings.eSyncOutMode[port] = ModeDivisor; - } - else if (tStr == "camera independent") - { - sCameraSettings.eSyncOutMode[port] = ModeIndependentFreq; - } - else if (tStr == "measurement time") - { - sCameraSettings.eSyncOutMode[port] = ModeMeasurementTime; - } - else if (tStr == "continuous 100hz") - { - sCameraSettings.eSyncOutMode[port] = ModeFixed100Hz; - } - else - { - return false; - } - - if (sCameraSettings.eSyncOutMode[port] == ModeMultiplier || - sCameraSettings.eSyncOutMode[port] == ModeDivisor || - sCameraSettings.eSyncOutMode[port] == ModeIndependentFreq) - { - if (!oXML.FindChildElem("Value")) - { - return false; - } - sCameraSettings.nSyncOutValue[port] = atoi(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Duty_Cycle")) - { - return false; - } - sCameraSettings.fSyncOutDutyCycle[port] = (float)atof(oXML.GetChildData().c_str()); - } - } - if (port == 2 || - (sCameraSettings.eSyncOutMode[port] != ModeFixed100Hz)) - { - if (!oXML.FindChildElem("Signal_Polarity")) - { - return false; - } - if (CompareNoCase(oXML.GetChildData(), "negative")) - { - sCameraSettings.bSyncOutNegativePolarity[port] = true; - } - else - { - sCameraSettings.bSyncOutNegativePolarity[port] = false; - } - } - oXML.OutOfElem(); // Sync_Out - } - else - { - sCameraSettings.eSyncOutMode[port] = ModeIndependentFreq; - sCameraSettings.nSyncOutValue[port] = 0; - sCameraSettings.fSyncOutDutyCycle[port] = 0; - sCameraSettings.bSyncOutNegativePolarity[port] = false; - } - } - - if (oXML.FindChildElem("LensControl")) - { - oXML.IntoElem(); - if (oXML.FindChildElem("Focus")) - { - oXML.IntoElem(); - float focus; - if (sscanf(oXML.GetAttrib("Value").c_str(), "%f", &focus) == 1) - { - sCameraSettings.fFocus = focus; - } - oXML.OutOfElem(); - } - if (oXML.FindChildElem("Aperture")) - { - oXML.IntoElem(); - float aperture; - if (sscanf(oXML.GetAttrib("Value").c_str(), "%f", &aperture) == 1) - { - sCameraSettings.fAperture = aperture; - } - oXML.OutOfElem(); - } - oXML.OutOfElem(); - } - else - { - sCameraSettings.fFocus = std::numeric_limits::quiet_NaN(); - sCameraSettings.fAperture = std::numeric_limits::quiet_NaN(); - } - - if (oXML.FindChildElem("AutoExposure")) - { - oXML.IntoElem(); - if (CompareNoCase(oXML.GetAttrib("Enabled"), "true")) - { - sCameraSettings.autoExposureEnabled = true; - } - float autoExposureCompensation; - if (sscanf(oXML.GetAttrib("Compensation").c_str(), "%f", &autoExposureCompensation) == 1) - { - sCameraSettings.autoExposureCompensation = autoExposureCompensation; - } - oXML.OutOfElem(); - } - else - { - sCameraSettings.autoExposureEnabled = false; - sCameraSettings.autoExposureCompensation = std::numeric_limits::quiet_NaN(); - } - - if (oXML.FindChildElem("AutoWhiteBalance")) - { - sCameraSettings.autoWhiteBalance = CompareNoCase(oXML.GetChildData().c_str(), "true") ? 1 : 0; - } - else - { - sCameraSettings.autoWhiteBalance = -1; - } - - oXML.OutOfElem(); // Camera - - msGeneralSettings.vsCameras.push_back(sCameraSettings); - } - - return true; -} // ReadGeneralSettings - - -bool ReadXmlFov(std::string name, CMarkup &oXML, CRTProtocol::SCalibrationFov &fov) -{ - if (!oXML.FindChildElem(name.c_str())) - { - return false; - } - fov.left = std::stoul(oXML.GetChildAttrib("left")); - fov.top = std::stoul(oXML.GetChildAttrib("top")); - fov.right = std::stoul(oXML.GetChildAttrib("right")); - fov.bottom = std::stoul(oXML.GetChildAttrib("bottom")); - - return true; -} - - -bool CRTProtocol::ReadCalibrationSettings() -{ - if (!SendCommand("GetParameters Calibration")) - { - strcpy(maErrorStr, "GetParameters Calibration failed"); - return false; - } - - return ReceiveCalibrationSettings(); -} - -bool CRTProtocol::ReceiveCalibrationSettings(int timeout) -{ - CRTPacket::EPacketType eType; - CMarkup oXML; - std::string tStr; - - SCalibration settings; - - auto received = ReceiveRTPacket(eType, true, timeout); - if (received <= 0) - { - if (received == 0) - { - // Receive timeout. - strcat(maErrorStr, " Expected XML packet."); - } - return false; - } - - if (eType != CRTPacket::PacketXML) - { - if (eType == CRTPacket::PacketError) - { - sprintf(maErrorStr, "%s.", mpoRTPacket->GetErrorString()); - } - else - { - sprintf(maErrorStr, "GetParameters Calibration returned wrong packet type. Got type %d expected type 2.", eType); - } - return false; - } - - oXML.SetDoc(mpoRTPacket->GetXMLString()); - - if (!oXML.FindChildElem("calibration")) - { - sprintf(maErrorStr, "Missing calibration element"); - return false; - } - oXML.IntoElem(); - - try - { - std::string resultStr = oXML.GetAttrib("calibrated"); - std::transform(resultStr.begin(), resultStr.end(), resultStr.begin(), ::tolower); - - settings.calibrated = (resultStr == "true"); - settings.source = oXML.GetAttrib("source"); - settings.created = oXML.GetAttrib("created"); - settings.qtm_version = oXML.GetAttrib("qtm-version"); - settings.type = oXML.GetAttrib("type"); - settings.wand_length = std::stod(oXML.GetAttrib("wandLength")); - settings.max_frames = std::stoul(oXML.GetAttrib("maximumFrames")); - settings.short_arm_end = std::stod(oXML.GetAttrib("shortArmEnd")); - settings.long_arm_end = std::stod(oXML.GetAttrib("longArmEnd")); - settings.long_arm_middle = std::stod(oXML.GetAttrib("longArmMiddle")); - - if (!oXML.FindChildElem("results")) - { - return false; - } - - settings.result_std_dev = std::stod(oXML.GetChildAttrib("std-dev")); - settings.result_min_max_diff = std::stod(oXML.GetChildAttrib("min-max-diff")); - - if (!oXML.FindChildElem("cameras")) - { - return false; - } - oXML.IntoElem(); - - while (oXML.FindChildElem("camera")) - { - oXML.IntoElem(); - SCalibrationCamera camera; - camera.active = std::stod(oXML.GetAttrib("active")) != 0; - - std::string calibratedStr = oXML.GetAttrib("calibrated"); - std::transform(calibratedStr.begin(), calibratedStr.end(), calibratedStr.begin(), ::tolower); - - camera.calibrated = (calibratedStr == "true"); - camera.message = oXML.GetAttrib("message"); - - camera.point_count = std::stoul(oXML.GetAttrib("point-count")); - camera.avg_residual = std::stod(oXML.GetAttrib("avg-residual")); - camera.serial = std::stoul(oXML.GetAttrib("serial")); - camera.model = oXML.GetAttrib("model"); - camera.view_rotation = std::stoul(oXML.GetAttrib("viewrotation")); - if (!ReadXmlFov("fov_marker", oXML, camera.fov_marker)) - { - return false; - } - if (!ReadXmlFov("fov_marker_max", oXML, camera.fov_marker_max)) - { - return false; - } - if (!ReadXmlFov("fov_video", oXML, camera.fov_video)) - { - return false; - } - if (!ReadXmlFov("fov_video_max", oXML, camera.fov_video_max)) - { - return false; - } - if (!oXML.FindChildElem("transform")) - { - return false; - } - camera.transform.x = std::stod(oXML.GetChildAttrib("x")); - camera.transform.y = std::stod(oXML.GetChildAttrib("y")); - camera.transform.z = std::stod(oXML.GetChildAttrib("z")); - camera.transform.r11 = std::stod(oXML.GetChildAttrib("r11")); - camera.transform.r12 = std::stod(oXML.GetChildAttrib("r12")); - camera.transform.r13 = std::stod(oXML.GetChildAttrib("r13")); - camera.transform.r21 = std::stod(oXML.GetChildAttrib("r21")); - camera.transform.r22 = std::stod(oXML.GetChildAttrib("r22")); - camera.transform.r23 = std::stod(oXML.GetChildAttrib("r23")); - camera.transform.r31 = std::stod(oXML.GetChildAttrib("r31")); - camera.transform.r32 = std::stod(oXML.GetChildAttrib("r32")); - camera.transform.r33 = std::stod(oXML.GetChildAttrib("r33")); - - if (!oXML.FindChildElem("intrinsic")) - { - return false; - } - - auto focalLength = oXML.GetChildAttrib("focallength"); - try - { - camera.intrinsic.focal_length = std::stod(focalLength); - } - catch (const std::invalid_argument&) - { - camera.intrinsic.focal_length = 0; - } - - camera.intrinsic.sensor_min_u = std::stod(oXML.GetChildAttrib("sensorMinU")); - camera.intrinsic.sensor_max_u = std::stod(oXML.GetChildAttrib("sensorMaxU")); - camera.intrinsic.sensor_min_v = std::stod(oXML.GetChildAttrib("sensorMinV")); - camera.intrinsic.sensor_max_v = std::stod(oXML.GetChildAttrib("sensorMaxV")); - camera.intrinsic.focal_length_u = std::stod(oXML.GetChildAttrib("focalLengthU")); - camera.intrinsic.focal_length_v = std::stod(oXML.GetChildAttrib("focalLengthV")); - camera.intrinsic.center_point_u = std::stod(oXML.GetChildAttrib("centerPointU")); - camera.intrinsic.center_point_v = std::stod(oXML.GetChildAttrib("centerPointV")); - camera.intrinsic.skew = std::stod(oXML.GetChildAttrib("skew")); - camera.intrinsic.radial_distortion_1 = std::stod(oXML.GetChildAttrib("radialDistortion1")); - camera.intrinsic.radial_distortion_2 = std::stod(oXML.GetChildAttrib("radialDistortion2")); - camera.intrinsic.radial_distortion_3 = std::stod(oXML.GetChildAttrib("radialDistortion3")); - camera.intrinsic.tangental_distortion_1 = std::stod(oXML.GetChildAttrib("tangentalDistortion1")); - camera.intrinsic.tangental_distortion_2 = std::stod(oXML.GetChildAttrib("tangentalDistortion2")); - oXML.OutOfElem(); // camera - settings.cameras.push_back(camera); - } - oXML.OutOfElem(); // cameras - } - catch (...) - { - return false; - } - - oXML.OutOfElem(); // calibration - - mCalibrationSettings = settings; - - return true; -} // ReadCalibrationSettings - - -bool CRTProtocol::Read3DSettings(bool &bDataAvailable) -{ - CRTPacket::EPacketType eType; - CMarkup oXML; - std::string tStr; - - bDataAvailable = false; - - ms3DSettings.s3DLabels.clear(); - ms3DSettings.pCalibrationTime[0] = 0; - - if (!SendCommand("GetParameters 3D")) - { - strcpy(maErrorStr, "GetParameters 3D failed"); - return false; - } - - auto received = ReceiveRTPacket(eType, true); - if (received <= 0) - { - if (received == 0) - { - // Receive timeout. - strcat(maErrorStr, " Expected XML packet."); - } - return false; - } - - if (eType != CRTPacket::PacketXML) - { - if (eType == CRTPacket::PacketError) - { - sprintf(maErrorStr, "%s.", mpoRTPacket->GetErrorString()); - } - else - { - sprintf(maErrorStr, "GetParameters 3D returned wrong packet type. Got type %d expected type 2.", eType); - } - return false; - } - - oXML.SetDoc(mpoRTPacket->GetXMLString()); - - if (!oXML.FindChildElem("The_3D")) - { - // No 3D data available. - return true; - } - oXML.IntoElem(); - - if (!oXML.FindChildElem("AxisUpwards")) - { - return false; - } - tStr = oXML.GetChildData(); - std::transform(tStr.begin(), tStr.end(), tStr.begin(), ::tolower); - - if (tStr == "+x") - { - ms3DSettings.eAxisUpwards = XPos; - } - else if (tStr == "-x") - { - ms3DSettings.eAxisUpwards = XNeg; - } - else if (tStr == "+y") - { - ms3DSettings.eAxisUpwards = YPos; - } - else if (tStr == "-y") - { - ms3DSettings.eAxisUpwards = YNeg; - } - else if (tStr == "+z") - { - ms3DSettings.eAxisUpwards = ZPos; - } - else if (tStr == "-z") - { - ms3DSettings.eAxisUpwards = ZNeg; - } - else - { - return false; - } - - if (!oXML.FindChildElem("CalibrationTime")) - { - return false; - } - tStr = oXML.GetChildData(); - strcpy(ms3DSettings.pCalibrationTime, tStr.c_str()); - - if (!oXML.FindChildElem("Labels")) - { - return false; - } - unsigned int nNumberOfLabels = atoi(oXML.GetChildData().c_str()); - - ms3DSettings.s3DLabels.resize(nNumberOfLabels); - SSettings3DLabel sLabel; - - for (unsigned int iLabel = 0; iLabel < nNumberOfLabels; iLabel++) - { - if (oXML.FindChildElem("Label")) - { - oXML.IntoElem(); - if (oXML.FindChildElem("Name")) - { - sLabel.oName = oXML.GetChildData(); - if (oXML.FindChildElem("RGBColor")) - { - sLabel.nRGBColor = atoi(oXML.GetChildData().c_str()); - } - ms3DSettings.s3DLabels[iLabel] = sLabel; - } - oXML.OutOfElem(); - } - else - { - return false; - } - } - - ms3DSettings.sBones.clear(); - if (oXML.FindChildElem("Bones")) - { - oXML.IntoElem(); - while (oXML.FindChildElem("Bone")) - { - oXML.IntoElem(); - SSettingsBone bone = { }; - bone.fromName = oXML.GetAttrib("From").c_str(); - bone.toName = oXML.GetAttrib("To").c_str(); - - auto colorString = oXML.GetAttrib("Color"); - if (!colorString.empty()) - { - bone.color = atoi(colorString.c_str()); - } - ms3DSettings.sBones.push_back(bone); - oXML.OutOfElem(); - } - oXML.OutOfElem(); - } - - bDataAvailable = true; - return true; -} // Read3DSettings - -bool CRTProtocol::Read6DOFSettings(bool &bDataAvailable) -{ - CRTPacket::EPacketType eType; - CMarkup oXML; - - bDataAvailable = false; - - mvs6DOFSettings.bodySettings.clear(); - - if (!SendCommand("GetParameters 6D")) - { - strcpy(maErrorStr, "GetParameters 6D failed"); - return false; - } - - auto received = ReceiveRTPacket(eType, true); - if (received <= 0) - { - if (received == 0) - { - // Receive timeout. - strcat(maErrorStr, " Expected XML packet."); - } - return false; - } - - if (eType != CRTPacket::PacketXML) - { - if (eType == CRTPacket::PacketError) - { - sprintf(maErrorStr, "%s.", mpoRTPacket->GetErrorString()); - } - else - { - sprintf(maErrorStr, "GetParameters 6D returned wrong packet type. Got type %d expected type 2.", eType); - } - return false; - } - - oXML.SetDoc(mpoRTPacket->GetXMLString()); - - // - // Read 6DOF bodies - // - if (!oXML.FindChildElem("The_6D")) - { - return true; // NO 6DOF data available. - } - oXML.IntoElem(); - - if (!oXML.FindChildElem("Bodies")) - { - return false; - } - int nBodies = atoi(oXML.GetChildData().c_str()); - SSettings6DOFBody s6DOFBodySettings; - SPoint sPoint; - - for (int iBody = 0; iBody < nBodies; iBody++) - { - if (!oXML.FindChildElem("Body")) - { - return false; - } - oXML.IntoElem(); - - if (!oXML.FindChildElem("Name")) - { - return false; - } - s6DOFBodySettings.oName = oXML.GetChildData(); - - if (!oXML.FindChildElem("RGBColor")) - { - return false; - } - s6DOFBodySettings.vsPoints.clear(); - s6DOFBodySettings.nRGBColor = atoi(oXML.GetChildData().c_str()); - - while (oXML.FindChildElem("Point")) - { - oXML.IntoElem(); - if (!oXML.FindChildElem("X")) - { - return false; - } - sPoint.fX = (float)atof(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Y")) - { - return false; - } - sPoint.fY = (float)atof(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Z")) - { - return false; - } - sPoint.fZ = (float)atof(oXML.GetChildData().c_str()); - - oXML.OutOfElem(); // Point - s6DOFBodySettings.vsPoints.push_back(sPoint); - } - mvs6DOFSettings.bodySettings.push_back(s6DOFBodySettings); - oXML.OutOfElem(); // Body - } - - if (mnMajorVersion > 1 || mnMinorVersion > 15) - { - if (oXML.FindChildElem("Euler")) - { - oXML.IntoElem(); - if (!oXML.FindChildElem("First")) - { - return false; - } - mvs6DOFSettings.eulerFirst = oXML.GetChildData(); - if (!oXML.FindChildElem("Second")) - { - return false; - } - mvs6DOFSettings.eulerSecond = oXML.GetChildData(); - if (!oXML.FindChildElem("Third")) - { - return false; - } - mvs6DOFSettings.eulerThird = oXML.GetChildData(); - oXML.OutOfElem(); // Euler - } - } - - bDataAvailable = true; - return true; -} // Read6DOFSettings - -bool CRTProtocol::ReadGazeVectorSettings(bool &bDataAvailable) -{ - CRTPacket::EPacketType eType; - CMarkup oXML; - - bDataAvailable = false; - - mvsGazeVectorSettings.clear(); - - if (!SendCommand("GetParameters GazeVector")) - { - strcpy(maErrorStr, "GetParameters GazeVector failed"); - return false; - } - - auto received = ReceiveRTPacket(eType, true); - if (received <= 0) - { - if (received == 0) - { - // Receive timeout. - strcat(maErrorStr, " Expected XML packet."); - } - return false; - } - - if (eType != CRTPacket::PacketXML) - { - if (eType == CRTPacket::PacketError) - { - sprintf(maErrorStr, "%s.", mpoRTPacket->GetErrorString()); - } - else - { - sprintf(maErrorStr, "GetParameters GazeVector returned wrong packet type. Got type %d expected type 2.", eType); - } - return false; - } - - oXML.SetDoc(mpoRTPacket->GetXMLString()); - - // - // Read gaze vectors - // - if (!oXML.FindChildElem("Gaze_Vector")) - { - return true; // NO gaze vector data available. - } - oXML.IntoElem(); - - std::string tGazeVectorName; - - int nGazeVectorCount = 0; - - while (oXML.FindChildElem("Vector")) - { - oXML.IntoElem(); - - if (!oXML.FindChildElem("Name")) - { - return false; - } - tGazeVectorName = oXML.GetChildData(); - - float frequency = 0; - if (oXML.FindChildElem("Frequency")) - { - frequency = (float)atof(oXML.GetChildData().c_str()); - } - - mvsGazeVectorSettings.push_back({ tGazeVectorName, frequency }); - nGazeVectorCount++; - oXML.OutOfElem(); // Vector - } - - bDataAvailable = true; - return true; -} // ReadGazeVectorSettings - -bool CRTProtocol::ReadAnalogSettings(bool &bDataAvailable) -{ - CRTPacket::EPacketType eType; - CMarkup oXML; - - bDataAvailable = false; - mvsAnalogDeviceSettings.clear(); - - if (!SendCommand("GetParameters Analog")) - { - strcpy(maErrorStr, "GetParameters Analog failed"); - return false; - } - - auto received = ReceiveRTPacket(eType, true); - if (received <= 0) - { - if (received == 0) - { - // Receive timeout. - strcat(maErrorStr, " Expected XML packet."); - } - return false; - } - - if (eType != CRTPacket::PacketXML) - { - if (eType == CRTPacket::PacketError) - { - sprintf(maErrorStr, "%s.", mpoRTPacket->GetErrorString()); - } - else - { - sprintf(maErrorStr, "GetParameters Analog returned wrong packet type. Got type %d expected type 2.", eType); - } - return false; - } - - oXML.SetDoc(mpoRTPacket->GetXMLString()); - - if (!oXML.FindChildElem("Analog")) - { - // No analog data available. - return true; - } - - SAnalogDevice sAnalogDevice; - - oXML.IntoElem(); - - if (mnMajorVersion == 1 && mnMinorVersion == 0) - { - sAnalogDevice.nDeviceID = 1; // Always channel 1 - sAnalogDevice.oName = "AnalogDevice"; - if (!oXML.FindChildElem("Channels")) - { - return false; - } - sAnalogDevice.nChannels = atoi(oXML.GetChildData().c_str()); - if (!oXML.FindChildElem("Frequency")) - { - return false; - } - sAnalogDevice.nFrequency = atoi(oXML.GetChildData().c_str()); - if (!oXML.FindChildElem("Unit")) - { - return false; - } - sAnalogDevice.oUnit = oXML.GetChildData(); - if (!oXML.FindChildElem("Range")) - { - return false; - } - oXML.IntoElem(); - if (!oXML.FindChildElem("Min")) - { - return false; - } - sAnalogDevice.fMinRange = (float)atof(oXML.GetChildData().c_str()); - if (!oXML.FindChildElem("Max")) - { - return false; - } - sAnalogDevice.fMaxRange = (float)atof(oXML.GetChildData().c_str()); - mvsAnalogDeviceSettings.push_back(sAnalogDevice); - bDataAvailable = true; - return true; - } - else - { - while (oXML.FindChildElem("Device")) - { - sAnalogDevice.voLabels.clear(); - sAnalogDevice.voUnits.clear(); - oXML.IntoElem(); - if (!oXML.FindChildElem("Device_ID")) - { - oXML.OutOfElem(); // Device - continue; - } - sAnalogDevice.nDeviceID = atoi(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Device_Name")) - { - oXML.OutOfElem(); // Device - continue; - } - sAnalogDevice.oName = oXML.GetChildData(); - - if (!oXML.FindChildElem("Channels")) - { - oXML.OutOfElem(); // Device - continue; - } - sAnalogDevice.nChannels = atoi(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Frequency")) - { - oXML.OutOfElem(); // Device - continue; - } - sAnalogDevice.nFrequency = atoi(oXML.GetChildData().c_str()); - - if (mnMajorVersion == 1 && mnMinorVersion < 11) - { - if (!oXML.FindChildElem("Unit")) - { - oXML.OutOfElem(); // Device - continue; - } - sAnalogDevice.oUnit = oXML.GetChildData(); - } - if (!oXML.FindChildElem("Range")) - { - oXML.OutOfElem(); // Device - continue; - } - oXML.IntoElem(); - - if (!oXML.FindChildElem("Min")) - { - oXML.OutOfElem(); // Device - oXML.OutOfElem(); // Range - continue; - } - sAnalogDevice.fMinRange = (float)atof(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Max")) - { - oXML.OutOfElem(); // Device - oXML.OutOfElem(); // Range - continue; - } - sAnalogDevice.fMaxRange = (float)atof(oXML.GetChildData().c_str()); - oXML.OutOfElem(); // Range - - if (mnMajorVersion == 1 && mnMinorVersion < 11) - { - for (unsigned int i = 0; i < sAnalogDevice.nChannels; i++) - { - if (oXML.FindChildElem("Label")) - { - sAnalogDevice.voLabels.push_back(oXML.GetChildData()); - } - } - if (sAnalogDevice.voLabels.size() != sAnalogDevice.nChannels) - { - oXML.OutOfElem(); // Device - continue; - } - } - else - { - while (oXML.FindChildElem("Channel")) - { - oXML.IntoElem(); - if (oXML.FindChildElem("Label")) - { - sAnalogDevice.voLabels.push_back(oXML.GetChildData()); - } - if (oXML.FindChildElem("Unit")) - { - sAnalogDevice.voUnits.push_back(oXML.GetChildData()); - } - oXML.OutOfElem(); // Channel - } - if (sAnalogDevice.voLabels.size() != sAnalogDevice.nChannels || - sAnalogDevice.voUnits.size() != sAnalogDevice.nChannels) - { - oXML.OutOfElem(); // Device - continue; - } - } - oXML.OutOfElem(); // Device - mvsAnalogDeviceSettings.push_back(sAnalogDevice); - bDataAvailable = true; - } - } - - return true; -} // ReadAnalogSettings - -bool CRTProtocol::ReadForceSettings(bool &bDataAvailable) -{ - CRTPacket::EPacketType eType; - CMarkup oXML; - - bDataAvailable = false; - - msForceSettings.vsForcePlates.clear(); - - if (!SendCommand("GetParameters Force")) - { - strcpy(maErrorStr, "GetParameters Force failed"); - return false; - } - - auto received = ReceiveRTPacket(eType, true); - if (received <= 0) - { - if (received == 0) - { - // Receive timeout. - strcat(maErrorStr, " Expected XML packet."); - } - return false; - } - - if (eType != CRTPacket::PacketXML) - { - if (eType == CRTPacket::PacketError) - { - sprintf(maErrorStr, "%s.", mpoRTPacket->GetErrorString()); - } - else - { - sprintf(maErrorStr, "GetParameters Force returned wrong packet type. Got type %d expected type 2.", eType); - } - return false; - } - - oXML.SetDoc(mpoRTPacket->GetXMLString()); - // - // Read some force plate parameters - // - if (!oXML.FindChildElem("Force")) - { - return true; - } - - oXML.IntoElem(); - - SForcePlate sForcePlate; - sForcePlate.bValidCalibrationMatrix = false; - sForcePlate.nCalibrationMatrixRows = 6; - sForcePlate.nCalibrationMatrixColumns = 6; - - if (!oXML.FindChildElem("Unit_Length")) - { - return false; - } - msForceSettings.oUnitLength = oXML.GetChildData(); - - if (!oXML.FindChildElem("Unit_Force")) - { - return false; - } - msForceSettings.oUnitForce = oXML.GetChildData(); - - int iPlate = 1; - while (oXML.FindChildElem("Plate")) - { - // - // Get name and type of the plates - // - oXML.IntoElem(); // "Plate" - if (oXML.FindChildElem("Force_Plate_Index")) // Version 1.7 and earlier. - { - sForcePlate.nID = atoi(oXML.GetChildData().c_str()); - } - else if (oXML.FindChildElem("Plate_ID")) // Version 1.8 and later. - { - sForcePlate.nID = atoi(oXML.GetChildData().c_str()); - } - else - { - return false; - } - - if (oXML.FindChildElem("Analog_Device_ID")) - { - sForcePlate.nAnalogDeviceID = atoi(oXML.GetChildData().c_str()); - } - else - { - sForcePlate.nAnalogDeviceID = 0; - } - - if (!oXML.FindChildElem("Frequency")) - { - return false; - } - sForcePlate.nFrequency = atoi(oXML.GetChildData().c_str()); - - if (oXML.FindChildElem("Type")) - { - sForcePlate.oType = oXML.GetChildData(); - } - else - { - sForcePlate.oType = "unknown"; - } - - if (oXML.FindChildElem("Name")) - { - sForcePlate.oName = oXML.GetChildData(); - } - else - { - sForcePlate.oName = CMarkup::Format("#%d", iPlate); - } - - if (oXML.FindChildElem("Length")) - { - sForcePlate.fLength = (float)atof(oXML.GetChildData().c_str()); - } - if (oXML.FindChildElem("Width")) - { - sForcePlate.fWidth = (float)atof(oXML.GetChildData().c_str()); - } - - if (oXML.FindChildElem("Location")) - { - oXML.IntoElem(); - if (oXML.FindChildElem("Corner1")) - { - oXML.IntoElem(); - if (oXML.FindChildElem("X")) - { - sForcePlate.asCorner[0].fX = (float)atof(oXML.GetChildData().c_str()); - } - if (oXML.FindChildElem("Y")) - { - sForcePlate.asCorner[0].fY = (float)atof(oXML.GetChildData().c_str()); - } - if (oXML.FindChildElem("Z")) - { - sForcePlate.asCorner[0].fZ = (float)atof(oXML.GetChildData().c_str()); - } - oXML.OutOfElem(); - } - if (oXML.FindChildElem("Corner2")) - { - oXML.IntoElem(); - if (oXML.FindChildElem("X")) - { - sForcePlate.asCorner[1].fX = (float)atof(oXML.GetChildData().c_str()); - } - if (oXML.FindChildElem("Y")) - { - sForcePlate.asCorner[1].fY = (float)atof(oXML.GetChildData().c_str()); - } - if (oXML.FindChildElem("Z")) - { - sForcePlate.asCorner[1].fZ = (float)atof(oXML.GetChildData().c_str()); - } - oXML.OutOfElem(); - } - if (oXML.FindChildElem("Corner3")) - { - oXML.IntoElem(); - if (oXML.FindChildElem("X")) - { - sForcePlate.asCorner[2].fX = (float)atof(oXML.GetChildData().c_str()); - } - if (oXML.FindChildElem("Y")) - { - sForcePlate.asCorner[2].fY = (float)atof(oXML.GetChildData().c_str()); - } - if (oXML.FindChildElem("Z")) - { - sForcePlate.asCorner[2].fZ = (float)atof(oXML.GetChildData().c_str()); - } - oXML.OutOfElem(); - } - if (oXML.FindChildElem("Corner4")) - { - oXML.IntoElem(); - if (oXML.FindChildElem("X")) - { - sForcePlate.asCorner[3].fX = (float)atof(oXML.GetChildData().c_str()); - } - if (oXML.FindChildElem("Y")) - { - sForcePlate.asCorner[3].fY = (float)atof(oXML.GetChildData().c_str()); - } - if (oXML.FindChildElem("Z")) - { - sForcePlate.asCorner[3].fZ = (float)atof(oXML.GetChildData().c_str()); - } - oXML.OutOfElem(); - } - oXML.OutOfElem(); - } - - if (oXML.FindChildElem("Origin")) - { - oXML.IntoElem(); - if (oXML.FindChildElem("X")) - { - sForcePlate.sOrigin.fX = (float)atof(oXML.GetChildData().c_str()); - } - if (oXML.FindChildElem("Y")) - { - sForcePlate.sOrigin.fY = (float)atof(oXML.GetChildData().c_str()); - } - if (oXML.FindChildElem("Z")) - { - sForcePlate.sOrigin.fZ = (float)atof(oXML.GetChildData().c_str()); - } - oXML.OutOfElem(); - } - - sForcePlate.vChannels.clear(); - if (oXML.FindChildElem("Channels")) - { - oXML.IntoElem(); - SForceChannel sForceChannel; - while (oXML.FindChildElem("Channel")) - { - oXML.IntoElem(); - if (oXML.FindChildElem("Channel_No")) - { - sForceChannel.nChannelNumber = atoi(oXML.GetChildData().c_str()); - } - if (oXML.FindChildElem("ConversionFactor")) - { - sForceChannel.fConversionFactor = (float)atof(oXML.GetChildData().c_str()); - } - sForcePlate.vChannels.push_back(sForceChannel); - oXML.OutOfElem(); - } - oXML.OutOfElem(); - } - - if (oXML.FindChildElem("Calibration_Matrix")) - { - oXML.IntoElem(); - int nRow = 0; - - if (mnMajorVersion == 1 && mnMinorVersion < 12) - { - char strRow[16]; - char strCol[16]; - sprintf(strRow, "Row%d", nRow + 1); - while (oXML.FindChildElem(strRow)) - { - oXML.IntoElem(); - int nCol = 0; - sprintf(strCol, "Col%d", nCol + 1); - while (oXML.FindChildElem(strCol)) - { - sForcePlate.afCalibrationMatrix[nRow][nCol] = (float)atof(oXML.GetChildData().c_str()); - nCol++; - sprintf(strCol, "Col%d", nCol + 1); - } - sForcePlate.nCalibrationMatrixColumns = nCol; - - nRow++; - sprintf(strRow, "Row%d", nRow + 1); - oXML.OutOfElem(); // RowX - } - } - else - { - // - if (oXML.FindChildElem("Rows")) - { - oXML.IntoElem(); - - while (oXML.FindChildElem("Row")) - { - oXML.IntoElem(); - - // - if (oXML.FindChildElem("Columns")) - { - oXML.IntoElem(); - - int nCol = 0; - while (oXML.FindChildElem("Column")) - { - sForcePlate.afCalibrationMatrix[nRow][nCol] = (float)atof(oXML.GetChildData().c_str()); - nCol++; - } - sForcePlate.nCalibrationMatrixColumns = nCol; - - nRow++; - oXML.OutOfElem(); // Columns - } - oXML.OutOfElem(); // Row - } - oXML.OutOfElem(); // Rows - } - } - sForcePlate.nCalibrationMatrixRows = nRow; - sForcePlate.bValidCalibrationMatrix = true; - - oXML.OutOfElem(); // "Calibration_Matrix" - } - oXML.OutOfElem(); // "Plate" - - bDataAvailable = true; - msForceSettings.vsForcePlates.push_back(sForcePlate); - } - - return true; -} // Read force settings - -bool CRTProtocol::ReadImageSettings(bool &bDataAvailable) -{ - CRTPacket::EPacketType eType; - CMarkup oXML; - - bDataAvailable = false; - - mvsImageSettings.clear(); - - if (!SendCommand("GetParameters Image")) - { - strcpy(maErrorStr, "GetParameters Image failed"); - return false; - } - - auto received = ReceiveRTPacket(eType, true); - if (received <= 0) - { - if (received == 0) - { - // Receive timeout. - strcat(maErrorStr, " Expected XML packet."); - } - return false; - } - - if (eType != CRTPacket::PacketXML) - { - if (eType == CRTPacket::PacketError) - { - sprintf(maErrorStr, "%s.", mpoRTPacket->GetErrorString()); - } - else - { - sprintf(maErrorStr, "GetParameters Image returned wrong packet type. Got type %d expected type 2.", eType); - } - return false; - } - - oXML.SetDoc(mpoRTPacket->GetXMLString()); - // - // Read some Image parameters - // - if (!oXML.FindChildElem("Image")) - { - return true; - } - oXML.IntoElem(); - - while (oXML.FindChildElem("Camera")) - { - oXML.IntoElem(); - - SImageCamera sImageCamera; - - if (!oXML.FindChildElem("ID")) - { - return false; - } - sImageCamera.nID = atoi(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Enabled")) - { - return false; - } - std::string tStr; - tStr = oXML.GetChildData(); - std::transform(tStr.begin(), tStr.end(), tStr.begin(), ::tolower); - - if (tStr == "true") - { - sImageCamera.bEnabled = true; - } - else - { - sImageCamera.bEnabled = false; - } - - if (!oXML.FindChildElem("Format")) - { - return false; - } - tStr = oXML.GetChildData(); - std::transform(tStr.begin(), tStr.end(), tStr.begin(), ::tolower); - - if (tStr == "rawgrayscale") - { - sImageCamera.eFormat = CRTPacket::FormatRawGrayscale; - } - else if (tStr == "rawbgr") - { - sImageCamera.eFormat = CRTPacket::FormatRawBGR; - } - else if (tStr == "jpg") - { - sImageCamera.eFormat = CRTPacket::FormatJPG; - } - else if (tStr == "png") - { - sImageCamera.eFormat = CRTPacket::FormatPNG; - } - else - { - return false; - } - - if (!oXML.FindChildElem("Width")) - { - return false; - } - sImageCamera.nWidth = atoi(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Height")) - { - return false; - } - sImageCamera.nHeight = atoi(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Left_Crop")) - { - return false; - } - sImageCamera.fCropLeft = (float)atof(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Top_Crop")) - { - return false; - } - sImageCamera.fCropTop = (float)atof(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Right_Crop")) - { - return false; - } - sImageCamera.fCropRight = (float)atof(oXML.GetChildData().c_str()); - - if (!oXML.FindChildElem("Bottom_Crop")) - { - return false; - } - sImageCamera.fCropBottom = (float)atof(oXML.GetChildData().c_str()); - - oXML.OutOfElem(); // "Camera" - - mvsImageSettings.push_back(sImageCamera); - bDataAvailable = true; - } - - return true; -} // ReadImageSettings - - -bool CRTProtocol::ReadSkeletonSettings(bool &bDataAvailable, bool skeletonGlobalData) -{ - CRTPacket::EPacketType eType; - CMarkup oXML; - - bDataAvailable = false; - - mSkeletonSettings.clear(); - - std::string cmd("GetParameters Skeleton"); - if (skeletonGlobalData) - { - cmd += ":global"; - } - if (!SendCommand(cmd.c_str())) - { - strcpy(maErrorStr, "GetParameters Skeleton failed"); - return false; - } - - auto received = ReceiveRTPacket(eType, true); - if (received <= 0) - { - if (received == 0) - { - // Receive timeout. - strcat(maErrorStr, " Expected XML packet."); - } - return false; - } - - if (eType != CRTPacket::PacketXML) - { - if (eType == CRTPacket::PacketError) - { - sprintf(maErrorStr, "%s.", mpoRTPacket->GetErrorString()); - } - else - { - sprintf(maErrorStr, "GetParameters Skeleton returned wrong packet type. Got type %d expected type 2.", eType); - } - return false; - } - - oXML.SetDoc(mpoRTPacket->GetXMLString()); - - if (!oXML.FindChildElem("Skeletons")) - { - return true; - } - oXML.IntoElem(); - - std::string skeletonName; - int segmentIndex; - std::map segmentIdIndexMap; - - while (oXML.FindChildElem("Skeleton")) - { - SSettingsSkeleton skeleton; - segmentIndex = 0; - - oXML.IntoElem(); - - skeleton.name = oXML.GetAttrib("Name"); - while (oXML.FindChildElem("Segment")) - { - oXML.IntoElem(); - - SSettingsSkeletonSegment segment; - - segment.name = oXML.GetAttrib("Name"); - if (segment.name.size() == 0 || sscanf(oXML.GetAttrib("ID").c_str(), "%u", &segment.id) != 1) - { - return false; - } - - segmentIdIndexMap[segment.id] = segmentIndex++; - - int parentId; - if (sscanf(oXML.GetAttrib("Parent_ID").c_str(), "%d", &parentId) != 1) - { - segment.parentId = -1; - segment.parentIndex = -1; - } - else if (segmentIdIndexMap.count(parentId) > 0) - { - segment.parentId = parentId; - segment.parentIndex = segmentIdIndexMap[parentId]; - } - - if (oXML.FindChildElem("Position")) - { - oXML.IntoElem(); - float x, y, z; - if (sscanf(oXML.GetAttrib("X").c_str(), "%f", &x) == 1) - { - segment.positionX = x; - } - if (sscanf(oXML.GetAttrib("Y").c_str(), "%f", &y) == 1) - { - segment.positionY = y; - } - if (sscanf(oXML.GetAttrib("Z").c_str(), "%f", &z) == 1) - { - segment.positionZ = z; - } - oXML.OutOfElem(); - } - - if (oXML.FindChildElem("Rotation")) - { - oXML.IntoElem(); - float x, y, z, w; - if (sscanf(oXML.GetAttrib("X").c_str(), "%f", &x) == 1) - { - segment.rotationX = x; - } - if (sscanf(oXML.GetAttrib("Y").c_str(), "%f", &y) == 1) - { - segment.rotationY = y; - } - if (sscanf(oXML.GetAttrib("Z").c_str(), "%f", &z) == 1) - { - segment.rotationZ = z; - } - if (sscanf(oXML.GetAttrib("W").c_str(), "%f", &w) == 1) - { - segment.rotationW = w; - } - oXML.OutOfElem(); - } - - skeleton.segments.push_back(segment); - - oXML.OutOfElem(); // Segment - } - - mSkeletonSettings.push_back(skeleton); - - oXML.OutOfElem(); // Skeleton - } - - oXML.OutOfElem(); // Skeletons - - bDataAvailable = true; - return true; -} // ReadSkeletonSettings - - -void CRTProtocol::GetSystemSettings( - unsigned int &nCaptureFrequency, float &fCaptureTime, - bool& bStartOnExtTrig, bool& startOnTrigNO, bool& startOnTrigNC, bool& startOnTrigSoftware, - EProcessingActions &eProcessingActions, EProcessingActions &eRtProcessingActions, EProcessingActions &eReprocessingActions) const -{ - nCaptureFrequency = msGeneralSettings.nCaptureFrequency; - fCaptureTime = msGeneralSettings.fCaptureTime; - bStartOnExtTrig = msGeneralSettings.bStartOnExternalTrigger; - startOnTrigNO = msGeneralSettings.bStartOnTrigNO; - startOnTrigNC = msGeneralSettings.bStartOnTrigNC; - startOnTrigSoftware = msGeneralSettings.bStartOnTrigSoftware; - eProcessingActions = msGeneralSettings.eProcessingActions; - eRtProcessingActions = msGeneralSettings.eRtProcessingActions; - eReprocessingActions = msGeneralSettings.eReprocessingActions; -} - - -void CRTProtocol::GetCalibrationSettings(SCalibration &calibrationSettings) const -{ - calibrationSettings = mCalibrationSettings; -} - - -// External time base settings only available in version 1.10 of the rt protocol and later -void CRTProtocol::GetExtTimeBaseSettings( - bool &bEnabled, ESignalSource &eSignalSource, - bool &bSignalModePeriodic, unsigned int &nFreqMultiplier, - unsigned int &nFreqDivisor, unsigned int &nFreqTolerance, - float &fNominalFrequency, bool &bNegativeEdge, - unsigned int &nSignalShutterDelay, float &fNonPeriodicTimeout) const -{ - bEnabled = msGeneralSettings.sExternalTimebase.bEnabled; - eSignalSource = msGeneralSettings.sExternalTimebase.eSignalSource; - bSignalModePeriodic = msGeneralSettings.sExternalTimebase.bSignalModePeriodic; - nFreqMultiplier = msGeneralSettings.sExternalTimebase.nFreqMultiplier; - nFreqDivisor = msGeneralSettings.sExternalTimebase.nFreqDivisor; - nFreqTolerance = msGeneralSettings.sExternalTimebase.nFreqTolerance; - fNominalFrequency = msGeneralSettings.sExternalTimebase.fNominalFrequency; - bNegativeEdge = msGeneralSettings.sExternalTimebase.bNegativeEdge; - nSignalShutterDelay = msGeneralSettings.sExternalTimebase.nSignalShutterDelay; - fNonPeriodicTimeout = msGeneralSettings.sExternalTimebase.fNonPeriodicTimeout; -} - - -unsigned int CRTProtocol::GetCameraCount() const -{ - return (unsigned int)msGeneralSettings.vsCameras.size(); -} - -std::vector CRTProtocol::GetDevices() const -{ - return msGeneralSettings.vsCameras; -} - - -bool CRTProtocol::GetCameraSettings( - unsigned int nCameraIndex, unsigned int &nID, ECameraModel &eModel, - bool &bUnderwater, bool &bSupportsHwSync, unsigned int &nSerial, ECameraMode &eMode) const -{ - if (nCameraIndex < msGeneralSettings.vsCameras.size()) - { - nID = msGeneralSettings.vsCameras[nCameraIndex].nID; - eModel = msGeneralSettings.vsCameras[nCameraIndex].eModel; - bUnderwater = msGeneralSettings.vsCameras[nCameraIndex].bUnderwater; - bSupportsHwSync = msGeneralSettings.vsCameras[nCameraIndex].bSupportsHwSync; - nSerial = msGeneralSettings.vsCameras[nCameraIndex].nSerial; - eMode = msGeneralSettings.vsCameras[nCameraIndex].eMode; - return true; - } - return false; -} - - -bool CRTProtocol::GetCameraMarkerSettings( - unsigned int nCameraIndex, unsigned int &nCurrentExposure, unsigned int &nMinExposure, - unsigned int &nMaxExposure, unsigned int &nCurrentThreshold, - unsigned int &nMinThreshold, unsigned int &nMaxThreshold) const -{ - if (nCameraIndex < msGeneralSettings.vsCameras.size()) - { - nCurrentExposure = msGeneralSettings.vsCameras[nCameraIndex].nMarkerExposure; - nMinExposure = msGeneralSettings.vsCameras[nCameraIndex].nMarkerExposureMin; - nMaxExposure = msGeneralSettings.vsCameras[nCameraIndex].nMarkerExposureMax; - nCurrentThreshold = msGeneralSettings.vsCameras[nCameraIndex].nMarkerThreshold; - nMinThreshold = msGeneralSettings.vsCameras[nCameraIndex].nMarkerThresholdMin; - nMaxThreshold = msGeneralSettings.vsCameras[nCameraIndex].nMarkerThresholdMax; - return true; - } - return false; -} - - -bool CRTProtocol::GetCameraVideoSettings( - unsigned int nCameraIndex, EVideoResolution &eVideoResolution, - EVideoAspectRatio &eVideoAspectRatio, unsigned int &nVideoFrequency, - unsigned int &nCurrentExposure, unsigned int &nMinExposure, - unsigned int &nMaxExposure, unsigned int &nCurrentFlashTime, - unsigned int &nMinFlashTime, unsigned int &nMaxFlashTime) const -{ - if (nCameraIndex < msGeneralSettings.vsCameras.size()) - { - eVideoResolution = msGeneralSettings.vsCameras[nCameraIndex].eVideoResolution; - eVideoAspectRatio = msGeneralSettings.vsCameras[nCameraIndex].eVideoAspectRatio; - nVideoFrequency = msGeneralSettings.vsCameras[nCameraIndex].nVideoFrequency; - nCurrentExposure = msGeneralSettings.vsCameras[nCameraIndex].nVideoExposure; - nMinExposure = msGeneralSettings.vsCameras[nCameraIndex].nVideoExposureMin; - nMaxExposure = msGeneralSettings.vsCameras[nCameraIndex].nVideoExposureMax; - nCurrentFlashTime = msGeneralSettings.vsCameras[nCameraIndex].nVideoFlashTime; - nMinFlashTime = msGeneralSettings.vsCameras[nCameraIndex].nVideoFlashTimeMin; - nMaxFlashTime = msGeneralSettings.vsCameras[nCameraIndex].nVideoFlashTimeMax; - return true; - } - return false; -} - - -bool CRTProtocol::GetCameraSyncOutSettings( - unsigned int nCameraIndex, unsigned int portNumber, ESyncOutFreqMode &eSyncOutMode, - unsigned int &nSyncOutValue, float &fSyncOutDutyCycle, - bool &bSyncOutNegativePolarity) const -{ - if (nCameraIndex < msGeneralSettings.vsCameras.size()) - { - if (portNumber == 1 || portNumber == 2) - { - eSyncOutMode = msGeneralSettings.vsCameras[nCameraIndex].eSyncOutMode[portNumber - 1]; - nSyncOutValue = msGeneralSettings.vsCameras[nCameraIndex].nSyncOutValue[portNumber - 1]; - fSyncOutDutyCycle = msGeneralSettings.vsCameras[nCameraIndex].fSyncOutDutyCycle[portNumber - 1]; - } - if (portNumber > 0 && portNumber < 4) - { - bSyncOutNegativePolarity = msGeneralSettings.vsCameras[nCameraIndex].bSyncOutNegativePolarity[portNumber - 1]; - } - else - { - return false; - } - return true; - } - return false; -} - - -bool CRTProtocol::GetCameraPosition( - unsigned int nCameraIndex, SPoint &sPoint, float fvRotationMatrix[3][3]) const -{ - if (nCameraIndex < msGeneralSettings.vsCameras.size()) - { - sPoint.fX = msGeneralSettings.vsCameras[nCameraIndex].fPositionX; - sPoint.fY = msGeneralSettings.vsCameras[nCameraIndex].fPositionY; - sPoint.fZ = msGeneralSettings.vsCameras[nCameraIndex].fPositionZ; - memcpy(fvRotationMatrix, msGeneralSettings.vsCameras[nCameraIndex].fPositionRotMatrix, 9 * sizeof(float)); - return true; - } - return false; -} - - -bool CRTProtocol::GetCameraOrientation( - unsigned int nCameraIndex, int &nOrientation) const -{ - if (nCameraIndex < msGeneralSettings.vsCameras.size()) - { - nOrientation = msGeneralSettings.vsCameras[nCameraIndex].nOrientation; - return true; - } - return false; -} - -bool CRTProtocol::GetCameraResolution( - unsigned int nCameraIndex, unsigned int &nMarkerWidth, unsigned int &nMarkerHeight, - unsigned int &nVideoWidth, unsigned int &nVideoHeight) const -{ - if (nCameraIndex < msGeneralSettings.vsCameras.size()) - { - nMarkerWidth = msGeneralSettings.vsCameras[nCameraIndex].nMarkerResolutionWidth; - nMarkerHeight = msGeneralSettings.vsCameras[nCameraIndex].nMarkerResolutionHeight; - nVideoWidth = msGeneralSettings.vsCameras[nCameraIndex].nVideoResolutionWidth; - nVideoHeight = msGeneralSettings.vsCameras[nCameraIndex].nVideoResolutionHeight; - return true; - } - return false; -} - -bool CRTProtocol::GetCameraFOV( - unsigned int nCameraIndex, unsigned int &nMarkerLeft, unsigned int &nMarkerTop, - unsigned int &nMarkerRight, unsigned int &nMarkerBottom, - unsigned int &nVideoLeft, unsigned int &nVideoTop, - unsigned int &nVideoRight, unsigned int &nVideoBottom) const -{ - if (nCameraIndex < msGeneralSettings.vsCameras.size()) - { - nMarkerLeft = msGeneralSettings.vsCameras[nCameraIndex].nMarkerFOVLeft; - nMarkerTop = msGeneralSettings.vsCameras[nCameraIndex].nMarkerFOVTop; - nMarkerRight = msGeneralSettings.vsCameras[nCameraIndex].nMarkerFOVRight; - nMarkerBottom = msGeneralSettings.vsCameras[nCameraIndex].nMarkerFOVBottom; - nVideoLeft = msGeneralSettings.vsCameras[nCameraIndex].nVideoFOVLeft; - nVideoTop = msGeneralSettings.vsCameras[nCameraIndex].nVideoFOVTop; - nVideoRight = msGeneralSettings.vsCameras[nCameraIndex].nVideoFOVRight; - nVideoBottom = msGeneralSettings.vsCameras[nCameraIndex].nVideoFOVBottom; - return true; - } - return false; -} - -bool CRTProtocol::GetCameraLensControlSettings(const unsigned int nCameraIndex, float* focus, float* aperture) const -{ - if (nCameraIndex < msGeneralSettings.vsCameras.size()) - { - *focus = msGeneralSettings.vsCameras[nCameraIndex].fFocus; - if (std::isnan(*focus)) - return false; - *aperture = msGeneralSettings.vsCameras[nCameraIndex].fAperture; - return true; - } - return false; -} - -bool CRTProtocol::GetCameraAutoExposureSettings(const unsigned int nCameraIndex, bool* autoExposureEnabled, float* autoExposureCompensation) const -{ - if (nCameraIndex < msGeneralSettings.vsCameras.size()) - { - *autoExposureCompensation = msGeneralSettings.vsCameras[nCameraIndex].autoExposureCompensation; - if (std::isnan(*autoExposureCompensation)) - return false; - *autoExposureEnabled = msGeneralSettings.vsCameras[nCameraIndex].autoExposureEnabled; - return true; - } - return false; -} - -bool CRTProtocol::GetCameraAutoWhiteBalance(const unsigned int nCameraIndex, bool* autoWhiteBalanceEnabled) const -{ - if (nCameraIndex < msGeneralSettings.vsCameras.size() && msGeneralSettings.vsCameras[nCameraIndex].autoWhiteBalance >= 0) - { - *autoWhiteBalanceEnabled = msGeneralSettings.vsCameras[nCameraIndex].autoWhiteBalance == 1; - return true; - } - return false; -} - -CRTProtocol::EAxis CRTProtocol::Get3DUpwardAxis() const -{ - return ms3DSettings.eAxisUpwards; -} - -const char* CRTProtocol::Get3DCalibrated() const -{ - return ms3DSettings.pCalibrationTime; -} - -unsigned int CRTProtocol::Get3DLabeledMarkerCount() const -{ - return (unsigned int)ms3DSettings.s3DLabels.size(); -} - -const char* CRTProtocol::Get3DLabelName(unsigned int nMarkerIndex) const -{ - if (nMarkerIndex < ms3DSettings.s3DLabels.size()) - { - return ms3DSettings.s3DLabels[nMarkerIndex].oName.c_str(); - } - return nullptr; -} - -unsigned int CRTProtocol::Get3DLabelColor(unsigned int nMarkerIndex) const -{ - if (nMarkerIndex < ms3DSettings.s3DLabels.size()) - { - return ms3DSettings.s3DLabels[nMarkerIndex].nRGBColor; - } - return 0; -} - - -unsigned int CRTProtocol::Get3DBoneCount() const -{ - return (unsigned int)ms3DSettings.sBones.size(); -} - -const char* CRTProtocol::Get3DBoneFromName(unsigned int boneIndex) const -{ - if (boneIndex < ms3DSettings.sBones.size()) - { - return ms3DSettings.sBones[boneIndex].fromName.c_str(); - } - return nullptr; -} - -const char* CRTProtocol::Get3DBoneToName(unsigned int boneIndex) const -{ - if (boneIndex < ms3DSettings.sBones.size()) - { - return ms3DSettings.sBones[boneIndex].toName.c_str(); - } - return nullptr; -} - -void CRTProtocol::Get6DOFEulerNames(std::string &first, std::string &second, std::string &third) const -{ - first = mvs6DOFSettings.eulerFirst; - second = mvs6DOFSettings.eulerSecond; - third = mvs6DOFSettings.eulerThird; -} - - -unsigned int CRTProtocol::Get6DOFBodyCount() const -{ - return (unsigned int)mvs6DOFSettings.bodySettings.size(); -} - - -const char* CRTProtocol::Get6DOFBodyName(unsigned int nBodyIndex) const -{ - if (nBodyIndex < mvs6DOFSettings.bodySettings.size()) - { - return mvs6DOFSettings.bodySettings[nBodyIndex].oName.c_str(); - } - return nullptr; -} - - -unsigned int CRTProtocol::Get6DOFBodyColor(unsigned int nBodyIndex) const -{ - if (nBodyIndex < mvs6DOFSettings.bodySettings.size()) - { - return mvs6DOFSettings.bodySettings[nBodyIndex].nRGBColor; - } - return 0; -} - - -unsigned int CRTProtocol::Get6DOFBodyPointCount(unsigned int nBodyIndex) const -{ - if (nBodyIndex < mvs6DOFSettings.bodySettings.size()) - { - return (unsigned int)mvs6DOFSettings.bodySettings.at(nBodyIndex).vsPoints.size(); - } - return false; -} - - -bool CRTProtocol::Get6DOFBodyPoint(unsigned int nBodyIndex, unsigned int nMarkerIndex, SPoint &sPoint) const -{ - if (nBodyIndex < mvs6DOFSettings.bodySettings.size()) - { - if (nMarkerIndex < mvs6DOFSettings.bodySettings.at(nBodyIndex).vsPoints.size()) - { - sPoint.fX = mvs6DOFSettings.bodySettings.at(nBodyIndex).vsPoints[nMarkerIndex].fX; - sPoint.fY = mvs6DOFSettings.bodySettings.at(nBodyIndex).vsPoints[nMarkerIndex].fY; - sPoint.fZ = mvs6DOFSettings.bodySettings.at(nBodyIndex).vsPoints[nMarkerIndex].fZ; - return true; - } - } - return false; -} - - -unsigned int CRTProtocol::GetGazeVectorCount() const -{ - return (unsigned int)mvsGazeVectorSettings.size(); -} - - -const char* CRTProtocol::GetGazeVectorName(unsigned int nGazeVectorIndex) const -{ - if (nGazeVectorIndex < mvsGazeVectorSettings.size()) - { - return mvsGazeVectorSettings[nGazeVectorIndex].name.c_str(); - } - return nullptr; -} - -float CRTProtocol::GetGazeVectorFrequency(unsigned int nGazeVectorIndex) const -{ - if (nGazeVectorIndex < mvsGazeVectorSettings.size()) - { - return mvsGazeVectorSettings[nGazeVectorIndex].frequency; - } - return 0; -} - - -unsigned int CRTProtocol::GetAnalogDeviceCount() const -{ - return (unsigned int)mvsAnalogDeviceSettings.size(); -} - - -bool CRTProtocol::GetAnalogDevice(unsigned int nDeviceIndex, unsigned int &nDeviceID, unsigned int &nChannels, - char* &pName, unsigned int &nFrequency, char* &pUnit, - float &fMinRange, float &fMaxRange) const -{ - if (nDeviceIndex < mvsAnalogDeviceSettings.size()) - { - nDeviceID = mvsAnalogDeviceSettings.at(nDeviceIndex).nDeviceID; - pName = (char*)mvsAnalogDeviceSettings.at(nDeviceIndex).oName.c_str(); - nChannels = mvsAnalogDeviceSettings.at(nDeviceIndex).nChannels; - nFrequency = mvsAnalogDeviceSettings.at(nDeviceIndex).nFrequency; - pUnit = (char*)mvsAnalogDeviceSettings.at(nDeviceIndex).oUnit.c_str(); - fMinRange = mvsAnalogDeviceSettings.at(nDeviceIndex).fMinRange; - fMaxRange = mvsAnalogDeviceSettings.at(nDeviceIndex).fMaxRange; - - return true; - } - return false; -} - - -const char* CRTProtocol::GetAnalogLabel(unsigned int nDeviceIndex, unsigned int nChannelIndex) const -{ - if (nDeviceIndex < mvsAnalogDeviceSettings.size()) - { - if (nChannelIndex < mvsAnalogDeviceSettings.at(nDeviceIndex).voLabels.size()) - { - return mvsAnalogDeviceSettings.at(nDeviceIndex).voLabels.at(nChannelIndex).c_str(); - } - } - return nullptr; -} - - -const char* CRTProtocol::GetAnalogUnit(unsigned int nDeviceIndex, unsigned int nChannelIndex) const -{ - if (nDeviceIndex < mvsAnalogDeviceSettings.size()) - { - if (nChannelIndex < mvsAnalogDeviceSettings.at(nDeviceIndex).voUnits.size()) - { - return mvsAnalogDeviceSettings.at(nDeviceIndex).voUnits.at(nChannelIndex).c_str(); - } - } - return nullptr; -} - - -void CRTProtocol::GetForceUnits(char* &pLength, char* &pForce) const -{ - pLength = (char*)msForceSettings.oUnitLength.c_str(); - pForce = (char*)msForceSettings.oUnitForce.c_str(); -} - - -unsigned int CRTProtocol::GetForcePlateCount() const -{ - return (unsigned int)msForceSettings.vsForcePlates.size(); -} - - -bool CRTProtocol::GetForcePlate(unsigned int nPlateIndex, unsigned int &nID, unsigned int &nAnalogDeviceID, - unsigned int &nFrequency, char* &pType, char* &pName, float &fLength, float &fWidth) const -{ - if (nPlateIndex < msForceSettings.vsForcePlates.size()) - { - nID = msForceSettings.vsForcePlates[nPlateIndex].nID; - nAnalogDeviceID = msForceSettings.vsForcePlates[nPlateIndex].nAnalogDeviceID; - nFrequency = msForceSettings.vsForcePlates[nPlateIndex].nFrequency; - pType = (char*)msForceSettings.vsForcePlates[nPlateIndex].oType.c_str(); - pName = (char*)msForceSettings.vsForcePlates[nPlateIndex].oName.c_str(); - fLength = msForceSettings.vsForcePlates[nPlateIndex].fLength; - fWidth = msForceSettings.vsForcePlates[nPlateIndex].fWidth; - return true; - } - return false; -} - - -bool CRTProtocol::GetForcePlateLocation(unsigned int nPlateIndex, SPoint sCorner[4]) const -{ - if (nPlateIndex < msForceSettings.vsForcePlates.size()) - { - memcpy(sCorner, msForceSettings.vsForcePlates[nPlateIndex].asCorner, 3 * 4 * sizeof(float)); - return true; - } - return false; -} - - -bool CRTProtocol::GetForcePlateOrigin(unsigned int nPlateIndex, SPoint &sOrigin) const -{ - if (nPlateIndex < msForceSettings.vsForcePlates.size()) - { - sOrigin = msForceSettings.vsForcePlates[nPlateIndex].sOrigin; - return true; - } - return false; -} - - -unsigned int CRTProtocol::GetForcePlateChannelCount(unsigned int nPlateIndex) const -{ - if (nPlateIndex < msForceSettings.vsForcePlates.size()) - { - return (unsigned int)msForceSettings.vsForcePlates[nPlateIndex].vChannels.size(); - } - return 0; -} - - -bool CRTProtocol::GetForcePlateChannel(unsigned int nPlateIndex, unsigned int nChannelIndex, - unsigned int &nChannelNumber, float &fConversionFactor) const -{ - if (nPlateIndex < msForceSettings.vsForcePlates.size()) - { - if (nChannelIndex < msForceSettings.vsForcePlates[nPlateIndex].vChannels.size()) - { - nChannelNumber = msForceSettings.vsForcePlates[nPlateIndex].vChannels[nChannelIndex].nChannelNumber; - fConversionFactor = msForceSettings.vsForcePlates[nPlateIndex].vChannels[nChannelIndex].fConversionFactor; - return true; - } - } - return false; -} - - -bool CRTProtocol::GetForcePlateCalibrationMatrix(unsigned int nPlateIndex, float fvCalMatrix[12][12], unsigned int* rows, unsigned int* columns) const -{ - if (nPlateIndex < msForceSettings.vsForcePlates.size()) - { - if (msForceSettings.vsForcePlates[nPlateIndex].bValidCalibrationMatrix) - { - *rows = msForceSettings.vsForcePlates[nPlateIndex].nCalibrationMatrixRows; - *columns = msForceSettings.vsForcePlates[nPlateIndex].nCalibrationMatrixColumns; - memcpy( - fvCalMatrix, - msForceSettings.vsForcePlates[nPlateIndex].afCalibrationMatrix, - msForceSettings.vsForcePlates[nPlateIndex].nCalibrationMatrixRows * msForceSettings.vsForcePlates[nPlateIndex].nCalibrationMatrixColumns * sizeof(float)); - return true; - } - } - return false; -} - - -unsigned int CRTProtocol::GetImageCameraCount() const -{ - return (unsigned int)mvsImageSettings.size(); -} - - -bool CRTProtocol::GetImageCamera(unsigned int nCameraIndex, unsigned int &nCameraID, bool &bEnabled, - CRTPacket::EImageFormat &eFormat, unsigned int &nWidth, unsigned int &nHeight, - float &fCropLeft, float &fCropTop, float &fCropRight, float &fCropBottom) const -{ - if (nCameraIndex < mvsImageSettings.size()) - { - nCameraID = mvsImageSettings[nCameraIndex].nID; - bEnabled = mvsImageSettings[nCameraIndex].bEnabled; - eFormat = mvsImageSettings[nCameraIndex].eFormat; - nWidth = mvsImageSettings[nCameraIndex].nWidth; - nHeight = mvsImageSettings[nCameraIndex].nHeight; - fCropLeft = mvsImageSettings[nCameraIndex].fCropLeft; - fCropTop = mvsImageSettings[nCameraIndex].fCropTop; - fCropRight = mvsImageSettings[nCameraIndex].fCropRight; - fCropBottom = mvsImageSettings[nCameraIndex].fCropBottom; - return true; - } - return false; -} - -unsigned int CRTProtocol::GetSkeletonCount() const -{ - return (unsigned int)mSkeletonSettings.size(); -} - - -const char* CRTProtocol::GetSkeletonName(unsigned int skeletonIndex) -{ - if (skeletonIndex < mSkeletonSettings.size()) - { - return (char*)mSkeletonSettings[skeletonIndex].name.c_str(); - } - return nullptr; -} - - -unsigned int CRTProtocol::GetSkeletonSegmentCount(unsigned int skeletonIndex) -{ - if (skeletonIndex < mSkeletonSettings.size()) - { - return static_cast(mSkeletonSettings[skeletonIndex].segments.size()); - } - return 0; -} - -bool CRTProtocol::GetSkeleton(unsigned int skeletonIndex, SSettingsSkeleton* skeleton) -{ - if (skeleton == nullptr) - return false; - - if (skeletonIndex < mSkeletonSettings.size()) - { - *skeleton = mSkeletonSettings[skeletonIndex]; - return true; - } - return false; -} - -bool CRTProtocol::GetSkeletonSegment(unsigned int skeletonIndex, unsigned int segmentIndex, SSettingsSkeletonSegment* segment) -{ - if (segment == nullptr) - return false; - - if (skeletonIndex < mSkeletonSettings.size()) - { - if (segmentIndex < mSkeletonSettings[skeletonIndex].segments.size()) - { - *segment = mSkeletonSettings[skeletonIndex].segments[segmentIndex]; - return true; - } - } - return false; -} - -bool CRTProtocol::SetSystemSettings( - const unsigned int* pnCaptureFrequency, const float* pfCaptureTime, - const bool* pbStartOnExtTrig, const bool* startOnTrigNO, const bool* startOnTrigNC, const bool* startOnTrigSoftware, - const EProcessingActions* peProcessingActions, const EProcessingActions* peRtProcessingActions, const EProcessingActions* peReprocessingActions) -{ - CMarkup oXML; - - oXML.AddElem("QTM_Settings"); - oXML.IntoElem(); - oXML.AddElem("General"); - oXML.IntoElem(); - - if (pnCaptureFrequency) - { - AddXMLElementUnsignedInt(&oXML, "Frequency", pnCaptureFrequency); - } - if (pfCaptureTime) - { - AddXMLElementFloat(&oXML, "Capture_Time", pfCaptureTime, 3); - } - if (pbStartOnExtTrig) - { - AddXMLElementBool(&oXML, "Start_On_External_Trigger", pbStartOnExtTrig); - if (mnMajorVersion > 1 || mnMinorVersion > 14) - { - AddXMLElementBool(&oXML, "Start_On_Trigger_NO", startOnTrigNO); - AddXMLElementBool(&oXML, "Start_On_Trigger_NC", startOnTrigNC); - AddXMLElementBool(&oXML, "Start_On_Trigger_Software", startOnTrigSoftware); - } - } - - const char* processings[3] = { "Processing_Actions", "RealTime_Processing_Actions", "Reprocessing_Actions" }; - const EProcessingActions* processingActions[3] = { peProcessingActions, peRtProcessingActions, peReprocessingActions }; - - auto actionsCount = (mnMajorVersion > 1 || mnMinorVersion > 13) ? 3 : 1; - - for (auto i = 0; i < actionsCount; i++) - { - if (processingActions[i]) - { - oXML.AddElem(processings[i]); - oXML.IntoElem(); - - if (mnMajorVersion > 1 || mnMinorVersion > 13) - { - AddXMLElementBool(&oXML, "PreProcessing2D", (*processingActions[i] & ProcessingPreProcess2D) != 0); - } - if (*processingActions[i] & ProcessingTracking2D && i != 1) // i != 1 => Not RtProcessingSettings - { - oXML.AddElem("Tracking", "2D"); - } - else if (*processingActions[i] & ProcessingTracking3D) - { - oXML.AddElem("Tracking", "3D"); - } - else - { - oXML.AddElem("Tracking", "False"); - } - if (i != 1) //Not RtProcessingSettings - { - AddXMLElementBool(&oXML, "TwinSystemMerge", (*processingActions[i] & ProcessingTwinSystemMerge) != 0); - AddXMLElementBool(&oXML, "SplineFill", (*processingActions[i] & ProcessingSplineFill) != 0); - } - AddXMLElementBool(&oXML, "AIM", (*processingActions[i] & ProcessingAIM) != 0); - AddXMLElementBool(&oXML, "Track6DOF", (*processingActions[i] & Processing6DOFTracking) != 0); - AddXMLElementBool(&oXML, "ForceData", (*processingActions[i] & ProcessingForceData) != 0); - AddXMLElementBool(&oXML, "GazeVector", (*processingActions[i] & ProcessingGazeVector) != 0); - if (i != 1) //Not RtProcessingSettings - { - AddXMLElementBool(&oXML, "ExportTSV", (*processingActions[i] & ProcessingExportTSV) != 0); - AddXMLElementBool(&oXML, "ExportC3D", (*processingActions[i] & ProcessingExportC3D) != 0); - AddXMLElementBool(&oXML, "ExportMatlabFile", (*processingActions[i] & ProcessingExportMatlabFile) != 0); - AddXMLElementBool(&oXML, "ExportAviFile", (*processingActions[i] & ProcessingExportAviFile) != 0); - } - oXML.OutOfElem(); // Processing_Actions - } - } - oXML.OutOfElem(); // General - oXML.OutOfElem(); // QTM_Settings - - if (SendXML(oXML.GetDoc().c_str())) - { - return true; - } - - return false; -} // SetGeneral - - -bool CRTProtocol::SetExtTimeBaseSettings( - const bool* pbEnabled, const ESignalSource* peSignalSource, - const bool* pbSignalModePeriodic, const unsigned int* pnFreqMultiplier, - const unsigned int* pnFreqDivisor, const unsigned int* pnFreqTolerance, - const float* pfNominalFrequency, const bool* pbNegativeEdge, - const unsigned int* pnSignalShutterDelay, const float* pfNonPeriodicTimeout) -{ - CMarkup oXML; - - oXML.AddElem("QTM_Settings"); - oXML.IntoElem(); - oXML.AddElem("General"); - oXML.IntoElem(); - oXML.AddElem("External_Time_Base"); - oXML.IntoElem(); - - AddXMLElementBool(&oXML, "Enabled", pbEnabled); - - if (peSignalSource) - { - switch (*peSignalSource) - { - case SourceControlPort : - oXML.AddElem("Signal_Source", "Control port"); - break; - case SourceIRReceiver : - oXML.AddElem("Signal_Source", "IR receiver"); - break; - case SourceSMPTE : - oXML.AddElem("Signal_Source", "SMPTE"); - break; - case SourceVideoSync : - oXML.AddElem("Signal_Source", "Video sync"); - break; - case SourceIRIG: - oXML.AddElem("Signal_Source", "IRIG"); - break; - } - } - - AddXMLElementBool(&oXML, "Signal_Mode", pbSignalModePeriodic, "Periodic", "Non-periodic"); - AddXMLElementUnsignedInt(&oXML, "Frequency_Multiplier", pnFreqMultiplier); - AddXMLElementUnsignedInt(&oXML, "Frequency_Divisor", pnFreqDivisor); - AddXMLElementUnsignedInt(&oXML, "Frequency_Tolerance", pnFreqTolerance); - - if (pfNominalFrequency) - { - if (*pfNominalFrequency < 0) - { - oXML.AddElem("Nominal_Frequency", "None"); - } - else - { - AddXMLElementFloat(&oXML, "Nominal_Frequency", pfNominalFrequency, 3); - } - } - - AddXMLElementBool(&oXML, "Signal_Edge", pbNegativeEdge, "Negative", "Positive"); - AddXMLElementUnsignedInt(&oXML, "Signal_Shutter_Delay", pnSignalShutterDelay); - AddXMLElementFloat(&oXML, "Non_Periodic_Timeout", pfNonPeriodicTimeout, 3); - - oXML.OutOfElem(); // External_Time_Base - oXML.OutOfElem(); // General - oXML.OutOfElem(); // QTM_Settings - - if (SendXML(oXML.GetDoc().c_str())) - { - return true; - } - - return false; -} // SetGeneralExtTimeBase - - -// nCameraID starts on 1. If nCameraID < 0 then settings are applied to all cameras. -bool CRTProtocol::SetCameraSettings( - const unsigned int nCameraID, const ECameraMode* peMode, - const float* pfMarkerExposure, const float* pfMarkerThreshold, - const int* pnOrientation) -{ - CMarkup oXML; - - oXML.AddElem("QTM_Settings"); - oXML.IntoElem(); - oXML.AddElem("General"); - oXML.IntoElem(); - - oXML.AddElem("Camera"); - oXML.IntoElem(); - - AddXMLElementUnsignedInt(&oXML, "ID", &nCameraID); - - if (peMode) - { - switch (*peMode) - { - case ModeMarker : - oXML.AddElem("Mode", "Marker"); - break; - case ModeMarkerIntensity : - oXML.AddElem("Mode", "Marker Intensity"); - break; - case ModeVideo : - oXML.AddElem("Mode", "Video"); - break; - } - } - AddXMLElementFloat(&oXML, "Marker_Exposure", pfMarkerExposure); - AddXMLElementFloat(&oXML, "Marker_Threshold", pfMarkerThreshold); - AddXMLElementInt(&oXML, "Orientation", pnOrientation); - - oXML.OutOfElem(); // Camera - oXML.OutOfElem(); // General - oXML.OutOfElem(); // QTM_Settings - - if (SendXML(oXML.GetDoc().c_str())) - { - return true; - } - - return false; -} // SetGeneralCamera - - -// nCameraID starts on 1. If nCameraID < 0 then settings are applied to all cameras. -bool CRTProtocol::SetCameraVideoSettings( - const unsigned int nCameraID, const EVideoResolution* eVideoResolution, - const EVideoAspectRatio* eVideoAspectRatio, const unsigned int* pnVideoFrequency, - const float* pfVideoExposure, const float* pfVideoFlashTime) -{ - CMarkup oXML; - - oXML.AddElem("QTM_Settings"); - oXML.IntoElem(); - oXML.AddElem("General"); - oXML.IntoElem(); - - oXML.AddElem("Camera"); - oXML.IntoElem(); - - AddXMLElementUnsignedInt(&oXML, "ID", &nCameraID); - if (eVideoResolution) - { - switch (*eVideoResolution) - { - case VideoResolution1080p: - oXML.AddElem("Video_Resolution", "1080p"); - break; - case VideoResolution720p: - oXML.AddElem("Video_Resolution", "720p"); - break; - case VideoResolution540p: - oXML.AddElem("Video_Resolution", "540p"); - break; - case VideoResolution480p: - oXML.AddElem("Video_Resolution", "480p"); - break; - case VideoResolutionNone: - break; - } - } - if (eVideoAspectRatio) - { - switch (*eVideoAspectRatio) - { - case VideoAspectRatio16x9: - oXML.AddElem("Video_Aspect_Ratio", "16x9"); - break; - case VideoAspectRatio4x3: - oXML.AddElem("Video_Aspect_Ratio", "4x3"); - break; - case VideoAspectRatio1x1: - oXML.AddElem("Video_Aspect_Ratio", "1x1"); - break; - case VideoAspectRatioNone: - break; - } - } - AddXMLElementUnsignedInt(&oXML, "Video_Frequency", pnVideoFrequency); - AddXMLElementFloat(&oXML, "Video_Exposure", pfVideoExposure); - AddXMLElementFloat(&oXML, "Video_Flash_Time", pfVideoFlashTime); - - oXML.OutOfElem(); // Camera - oXML.OutOfElem(); // General - oXML.OutOfElem(); // QTM_Settings - - if (SendXML(oXML.GetDoc().c_str())) - { - return true; - } - - return false; -} // SetGeneralCameraVideo - - -// nCameraID starts on 1. If nCameraID < 0 then settings are applied to all cameras. -bool CRTProtocol::SetCameraSyncOutSettings( - const unsigned int nCameraID, const unsigned int portNumber, const ESyncOutFreqMode* peSyncOutMode, - const unsigned int* pnSyncOutValue, const float* pfSyncOutDutyCycle, - const bool* pbSyncOutNegativePolarity) -{ - CMarkup oXML; - - oXML.AddElem("QTM_Settings"); - oXML.IntoElem(); - oXML.AddElem("General"); - oXML.IntoElem(); - - oXML.AddElem("Camera"); - oXML.IntoElem(); - - AddXMLElementUnsignedInt(&oXML, "ID", &nCameraID); - - int port = portNumber - 1; - if (((port == 0 || port == 1) && peSyncOutMode) || (port == 2)) - { - oXML.AddElem(port == 0 ? "Sync_Out" : (port == 1 ? "Sync_Out2" : "Sync_Out_MT")); - oXML.IntoElem(); - - if (port == 0 || port == 1) - { - switch (*peSyncOutMode) - { - case ModeShutterOut: - oXML.AddElem("Mode", "Shutter out"); - break; - case ModeMultiplier: - oXML.AddElem("Mode", "Multiplier"); - break; - case ModeDivisor: - oXML.AddElem("Mode", "Divisor"); - break; - case ModeIndependentFreq: - oXML.AddElem("Mode", "Camera independent"); - break; - case ModeMeasurementTime: - oXML.AddElem("Mode", "Measurement time"); - break; - case ModeFixed100Hz: - oXML.AddElem("Mode", "Continuous 100Hz"); - break; - default: - return false; // Should never happen - } - - if (*peSyncOutMode == ModeMultiplier || - *peSyncOutMode == ModeDivisor || - *peSyncOutMode == ModeIndependentFreq) - { - if (pnSyncOutValue) - { - AddXMLElementUnsignedInt(&oXML, "Value", pnSyncOutValue); - } - if (pfSyncOutDutyCycle) - { - AddXMLElementFloat(&oXML, "Duty_Cycle", pfSyncOutDutyCycle, 3); - } - } - } - if (pbSyncOutNegativePolarity && (port == 2 || - (peSyncOutMode && *peSyncOutMode != ModeFixed100Hz))) - { - AddXMLElementBool(&oXML, "Signal_Polarity", pbSyncOutNegativePolarity, "Negative", "Positive"); - } - oXML.OutOfElem(); // Sync_Out - } - oXML.OutOfElem(); // Camera - oXML.OutOfElem(); // General - oXML.OutOfElem(); // QTM_Settings - - if (SendXML(oXML.GetDoc().c_str())) - { - return true; - } - - return false; -} // SetGeneralCameraSyncOut - - - // nCameraID starts on 1. If nCameraID < 0 then settings are applied to all cameras. -bool CRTProtocol::SetCameraLensControlSettings(const unsigned int nCameraID, const float focus, const float aperture) -{ - CMarkup oXML; - - oXML.AddElem("QTM_Settings"); - oXML.IntoElem(); - oXML.AddElem("General"); - oXML.IntoElem(); - - oXML.AddElem("Camera"); - oXML.IntoElem(); - - AddXMLElementUnsignedInt(&oXML, "ID", &nCameraID); - - oXML.AddElem("LensControl"); - oXML.IntoElem(); - - oXML.AddElem("Focus"); - oXML.AddAttrib("Value", CMarkup::Format("%f", focus).c_str()); - oXML.AddElem("Aperture"); - oXML.AddAttrib("Value", CMarkup::Format("%f", aperture).c_str()); - - oXML.OutOfElem(); // LensControl - oXML.OutOfElem(); // Camera - oXML.OutOfElem(); // General - oXML.OutOfElem(); // QTM_Settings - - if (SendXML(oXML.GetDoc().c_str())) - { - return true; - } - - return false; -} - -// nCameraID starts on 1. If nCameraID < 0 then settings are applied to all cameras. -bool CRTProtocol::SetCameraAutoExposureSettings(const unsigned int nCameraID, const bool autoExposure, const float compensation) -{ - CMarkup oXML; - - oXML.AddElem("QTM_Settings"); - oXML.IntoElem(); - oXML.AddElem("General"); - oXML.IntoElem(); - - oXML.AddElem("Camera"); - oXML.IntoElem(); - - AddXMLElementUnsignedInt(&oXML, "ID", &nCameraID); - - oXML.AddElem("LensControl"); - oXML.IntoElem(); - - oXML.AddElem("AutoExposure"); - oXML.AddAttrib("Enabled", autoExposure ? "true" : "false"); - oXML.AddAttrib("Compensation", CMarkup::Format("%f", compensation).c_str()); - - oXML.OutOfElem(); // AutoExposure - oXML.OutOfElem(); // Camera - oXML.OutOfElem(); // General - oXML.OutOfElem(); // QTM_Settings - - if (SendXML(oXML.GetDoc().c_str())) - { - return true; - } - - return false; -} - -// nCameraID starts on 1. If nCameraID < 0 then settings are applied to all cameras. -bool CRTProtocol::SetCameraAutoWhiteBalance(const unsigned int nCameraID, const bool enable) -{ - CMarkup oXML; - - oXML.AddElem("QTM_Settings"); - oXML.IntoElem(); - oXML.AddElem("General"); - oXML.IntoElem(); - - oXML.AddElem("Camera"); - oXML.IntoElem(); - - AddXMLElementUnsignedInt(&oXML, "ID", &nCameraID); - - oXML.AddElem("AutoWhiteBalance", enable ? "true" : "false"); - - oXML.OutOfElem(); // Camera - oXML.OutOfElem(); // General - oXML.OutOfElem(); // QTM_Settings - - if (SendXML(oXML.GetDoc().c_str())) - { - return true; - } - - return false; -} - - -bool CRTProtocol::SetImageSettings( - const unsigned int nCameraID, const bool* pbEnable, const CRTPacket::EImageFormat* peFormat, - const unsigned int* pnWidth, const unsigned int* pnHeight, const float* pfLeftCrop, - const float* pfTopCrop, const float* pfRightCrop, const float* pfBottomCrop) -{ - CMarkup oXML; - - oXML.AddElem("QTM_Settings"); - oXML.IntoElem(); - oXML.AddElem("Image"); - oXML.IntoElem(); - - oXML.AddElem("Camera"); - oXML.IntoElem(); - - AddXMLElementUnsignedInt(&oXML, "ID", &nCameraID); - - AddXMLElementBool(&oXML, "Enabled", pbEnable); - - if (peFormat) - { - switch (*peFormat) - { - case CRTPacket::FormatRawGrayscale : - oXML.AddElem("Format", "RAWGrayscale"); - break; - case CRTPacket::FormatRawBGR : - oXML.AddElem("Format", "RAWBGR"); - break; - case CRTPacket::FormatJPG : - oXML.AddElem("Format", "JPG"); - break; - case CRTPacket::FormatPNG : - oXML.AddElem("Format", "PNG"); - break; - } - } - AddXMLElementUnsignedInt(&oXML, "Width", pnWidth); - AddXMLElementUnsignedInt(&oXML, "Height", pnHeight); - AddXMLElementFloat(&oXML, "Left_Crop", pfLeftCrop); - AddXMLElementFloat(&oXML, "Top_Crop", pfTopCrop); - AddXMLElementFloat(&oXML, "Right_Crop", pfRightCrop); - AddXMLElementFloat(&oXML, "Bottom_Crop", pfBottomCrop); - - oXML.OutOfElem(); // Camera - oXML.OutOfElem(); // Image - oXML.OutOfElem(); // QTM_Settings - - if (SendXML(oXML.GetDoc().c_str())) - { - return true; - } - - return false; -} // SetImageSettings - - -bool CRTProtocol::SetForceSettings( - const unsigned int nPlateID, const SPoint* psCorner1, const SPoint* psCorner2, - const SPoint* psCorner3, const SPoint* psCorner4) -{ - CMarkup oXML; - - if (nPlateID > 0) - { - oXML.AddElem("QTM_Settings"); - oXML.IntoElem(); - oXML.AddElem("Force"); - oXML.IntoElem(); - - oXML.AddElem("Plate"); - oXML.IntoElem(); - - if (mnMajorVersion > 1 || mnMinorVersion > 7) - { - AddXMLElementUnsignedInt(&oXML, "Plate_ID", &nPlateID); - } - else - { - AddXMLElementUnsignedInt(&oXML, "Force_Plate_Index", &nPlateID); - } - if (psCorner1) - { - oXML.AddElem("Corner1"); - oXML.IntoElem(); - AddXMLElementFloat(&oXML, "X", &(psCorner1->fX)); - AddXMLElementFloat(&oXML, "Y", &(psCorner1->fY)); - AddXMLElementFloat(&oXML, "Z", &(psCorner1->fZ)); - oXML.OutOfElem(); // Corner1 - } - if (psCorner2) - { - oXML.AddElem("Corner2"); - oXML.IntoElem(); - AddXMLElementFloat(&oXML, "X", &(psCorner2->fX)); - AddXMLElementFloat(&oXML, "Y", &(psCorner2->fY)); - AddXMLElementFloat(&oXML, "Z", &(psCorner2->fZ)); - oXML.OutOfElem(); // Corner2 - } - if (psCorner3) - { - oXML.AddElem("Corner3"); - oXML.IntoElem(); - AddXMLElementFloat(&oXML, "X", &(psCorner3->fX)); - AddXMLElementFloat(&oXML, "Y", &(psCorner3->fY)); - AddXMLElementFloat(&oXML, "Z", &(psCorner3->fZ)); - oXML.OutOfElem(); // Corner3 - } - if (psCorner4) - { - oXML.AddElem("Corner4"); - oXML.IntoElem(); - AddXMLElementFloat(&oXML, "X", &(psCorner4->fX)); - AddXMLElementFloat(&oXML, "Y", &(psCorner4->fY)); - AddXMLElementFloat(&oXML, "Z", &(psCorner4->fZ)); - oXML.OutOfElem(); // Corner4 - } - oXML.OutOfElem(); // Plate - - oXML.OutOfElem(); // Force - oXML.OutOfElem(); // QTM_Settings - - if (SendXML(oXML.GetDoc().c_str())) - { - return true; - } - } - else - { - sprintf(maErrorStr, "Illegal force plate id: %d.", nPlateID); - } - return false; -} // SetForceSettings - - -char* CRTProtocol::GetErrorString() -{ - return maErrorStr; -} - - -bool CRTProtocol::SendString(const char* pCmdStr, int nType) -{ - size_t nSize; - size_t nCmdStrLen = strlen(pCmdStr); - - if (nCmdStrLen > sizeof(mSendBuffer)) - { - strcpy(maErrorStr, "String is larger than send buffer."); - return false; - } - - // - // Header size + length of the string + terminating null char - // - nSize = 8 + nCmdStrLen + 1; - - memcpy(mSendBuffer + 8, pCmdStr, nCmdStrLen + 1); - - if ((mnMajorVersion == 1 && mnMinorVersion == 0) || mbBigEndian) - { - *((unsigned int*)mSendBuffer) = htonl(nSize); - *((unsigned int*)(mSendBuffer + 4)) = htonl(nType); - } - else - { - *((unsigned int*)mSendBuffer) = nSize; - *((unsigned int*)(mSendBuffer + 4)) = nType; - } - - if (mpoNetwork->Send(mSendBuffer, nSize) == false) - { - strcpy(maErrorStr, mpoNetwork->GetErrorString()); - return false; - } - - return true; -} // SendString - - -bool CRTProtocol::SendCommand(const char* pCmdStr) -{ - return SendString(pCmdStr, CRTPacket::PacketCommand); -} // SendCommand - - -bool CRTProtocol::SendCommand(const char* pCmdStr, char* pCommandResponseStr, unsigned int timeout) -{ - CRTPacket::EPacketType eType; - - if (SendString(pCmdStr, CRTPacket::PacketCommand)) - { - while (ReceiveRTPacket(eType, true, timeout) > 0) - { - if (eType == CRTPacket::PacketCommand) - { - strcpy(pCommandResponseStr, mpoRTPacket->GetCommandString()); - return true; - } - if (eType == CRTPacket::PacketError) - { - strcpy(pCommandResponseStr, mpoRTPacket->GetErrorString()); - return false; - } - } - } - else - { - char pTmpStr[256]; - strcpy(pTmpStr, maErrorStr); - sprintf(maErrorStr, "\'%s\' command failed. %s", pCmdStr, pTmpStr); - } - pCommandResponseStr[0] = 0; - return false; -} // SendCommand - - -bool CRTProtocol::SendXML(const char* pCmdStr) -{ - CRTPacket::EPacketType eType; - - if (SendString(pCmdStr, CRTPacket::PacketXML)) - { - if (ReceiveRTPacket(eType, true) > 0) - { - if (eType == CRTPacket::PacketCommand) - { - if (strcmp(mpoRTPacket->GetCommandString(), "Setting parameters succeeded") == 0) - { - return true; - } - else - { - sprintf(maErrorStr, - "Expected command response \"Setting parameters succeeded\". Got \"%s\".", - mpoRTPacket->GetCommandString()); - } - } - else - { - sprintf(maErrorStr, "Expected command response packet. Got packet type %d.", (int)eType); - } - } - else - { - strcpy(maErrorStr, "Missing command response packet."); - } - } - else - { - char pTmpStr[256]; - strcpy(pTmpStr, maErrorStr); - sprintf(maErrorStr, "Failed to send XML string. %s", pTmpStr); - } - return false; -} // SendXML - - -void CRTProtocol::AddXMLElementBool(CMarkup* oXML, const char* tTag, const bool* pbValue, const char* tTrue, const char* tFalse) -{ - if (pbValue) - { - oXML->AddElem(tTag, *pbValue ? tTrue : tFalse); - } -} - - -void CRTProtocol::AddXMLElementBool(CMarkup* oXML, const char* tTag, const bool pbValue, const char* tTrue, const char* tFalse) -{ - oXML->AddElem(tTag, pbValue ? tTrue : tFalse); -} - - -void CRTProtocol::AddXMLElementInt(CMarkup* oXML, const char* tTag, const int* pnValue) -{ - if (pnValue) - { - std::string tVal; - - tVal = CMarkup::Format("%d", *pnValue); - oXML->AddElem(tTag, tVal.c_str()); - } -} - - -void CRTProtocol::AddXMLElementUnsignedInt(CMarkup* oXML, const char* tTag, const unsigned int* pnValue) -{ - if (pnValue) - { - std::string tVal; - - tVal = CMarkup::Format("%u", *pnValue); - oXML->AddElem(tTag, tVal.c_str()); - } -} - -void CRTProtocol::AddXMLElementFloat(CMarkup* oXML, const char* tTag, const float* pfValue, unsigned int pnDecimals) -{ - if (pfValue) - { - std::string tVal; - char fFormat[10]; - - sprintf(fFormat, "%%.%df", pnDecimals); - tVal = CMarkup::Format(fFormat, *pfValue); - oXML->AddElem(tTag, tVal.c_str()); - } -} - -bool CRTProtocol::CompareNoCase(std::string tStr1, const char* tStr2) const -{ - std::transform(tStr1.begin(), tStr1.end(), tStr1.begin(), ::tolower); - return tStr1.compare(tStr2) == 0; -} \ No newline at end of file diff --git a/navigation/qualisys_motion_capture/mocap_qualisys/include/mocap_qualisys/RTProtocol.h b/navigation/qualisys_motion_capture/mocap_qualisys/include/mocap_qualisys/RTProtocol.h deleted file mode 100644 index 8ad9615e1..000000000 --- a/navigation/qualisys_motion_capture/mocap_qualisys/include/mocap_qualisys/RTProtocol.h +++ /dev/null @@ -1,752 +0,0 @@ -#ifndef RTPROTOCOL_H -#define RTPROTOCOL_H - - -#include "RTPacket.h" -#include "Network.h" -#include -#include -#include - -#ifdef _WIN32 -#pragma warning (disable : 4251) -#endif - - -#ifdef EXPORT_DLL -#define DLL_EXPORT __declspec(dllexport) -#else -#define DLL_EXPORT -#endif - - -//#define DEFAULT_AUTO_DESCOVER_PORT 22226 -//#define WAIT_FOR_DATA_TIMEOUT 5000000 // 5 s - -class CMarkup; - -class DLL_EXPORT CRTProtocol -{ -public: - static const unsigned int cDefaultBasePort = 22222; - static const unsigned int cDefaultTelnetPort = cDefaultBasePort - 1; - static const unsigned int cDefaultLegacyPort = cDefaultBasePort; // Only supports version 1.0 of the protocol. - static const unsigned int cDefaultLittleEndianPort = cDefaultBasePort + 1; - static const unsigned int cDefaultBigEndianPort = cDefaultBasePort + 2; - static const unsigned int cDefaultOscPort = cDefaultBasePort + 3; - static const unsigned int cDefaultAutoDescoverPort = cDefaultBasePort + 4; - - static const unsigned int cWaitForDataTimeout = 5000000; // 5 s - static const unsigned int cWaitForCalibrationTimeout = 600000000; // 10 min - - static const unsigned int cComponent3d = 0x000001; - static const unsigned int cComponent3dNoLabels = 0x000002; - static const unsigned int cComponentAnalog = 0x000004; - static const unsigned int cComponentForce = 0x000008; - static const unsigned int cComponent6d = 0x000010; - static const unsigned int cComponent6dEuler = 0x000020; - static const unsigned int cComponent2d = 0x000040; - static const unsigned int cComponent2dLin = 0x000080; - static const unsigned int cComponent3dRes = 0x000100; - static const unsigned int cComponent3dNoLabelsRes = 0x000200; - static const unsigned int cComponent6dRes = 0x000400; - static const unsigned int cComponent6dEulerRes = 0x000800; - static const unsigned int cComponentAnalogSingle = 0x001000; - static const unsigned int cComponentImage = 0x002000; - static const unsigned int cComponentForceSingle = 0x004000; - static const unsigned int cComponentGazeVector = 0x008000; - static const unsigned int cComponentTimecode = 0x010000; - static const unsigned int cComponentSkeleton = 0x020000; - - struct SComponentOptions - { - SComponentOptions() : - mAnalogChannels(nullptr), - mSkeletonGlobalData(false) - { - } - char* mAnalogChannels; - bool mSkeletonGlobalData; - }; - - enum EStreamRate - { - RateNone = 0, - RateAllFrames = 1, - RateFrequency = 2, - RateFrequencyDivisor = 3 - }; - - enum ECameraModel - { - ModelMacReflex = 0, - ModelProReflex120 = 1, - ModelProReflex240 = 2, - ModelProReflex500 = 3, - ModelProReflex1000 = 4, - ModelOqus100 = 5, - ModelOqus300 = 6, - ModelOqus300Plus = 7, - ModelOqus400 = 8, - ModelOqus500 = 9, - ModelOqus200C = 10, - ModelOqus500Plus = 11, - ModelOqus700 = 12, - ModelOqus700Plus = 13, - ModelOqus600Plus = 14, - ModelMiqusM1 = 15, - ModelMiqusM3 = 16, - ModelMiqusM5 = 17, - ModelMiqusSyncUnit = 18, - ModelMiqusVideo = 19, - ModelMiqusVideoColor = 20 - }; - - enum ECameraMode - { - ModeMarker = 0, - ModeMarkerIntensity = 1, - ModeVideo = 2 - }; - - enum EVideoResolution - { - VideoResolution1080p = 0, - VideoResolution720p = 1, - VideoResolution540p = 2, - VideoResolution480p = 3, - VideoResolutionNone = 4 - }; - - enum EVideoAspectRatio - { - VideoAspectRatio16x9 = 0, - VideoAspectRatio4x3 = 1, - VideoAspectRatio1x1 = 2, - VideoAspectRatioNone = 3 - }; - - enum ESyncOutFreqMode - { - ModeShutterOut = 1, // A pulse per frame is sent - ModeMultiplier, - ModeDivisor, - ModeIndependentFreq, - ModeMeasurementTime, - ModeFixed100Hz - }; - - enum ESignalSource - { - SourceControlPort = 0, - SourceIRReceiver = 1, - SourceSMPTE = 2, - SourceVideoSync = 3, - SourceIRIG = 4 - }; - - enum EAxis - { - XPos = 0, - XNeg = 1, - YPos = 2, - YNeg = 3, - ZPos = 4, - ZNeg = 5 - }; - - enum EProcessingActions - { - ProcessingNone = 0x0000, - ProcessingTracking2D = 0x0001, - ProcessingTracking3D = 0x0002, - ProcessingTwinSystemMerge = 0x0004, - ProcessingSplineFill = 0x0008, - ProcessingAIM = 0x0010, - Processing6DOFTracking = 0x0020, - ProcessingForceData = 0x0040, - ProcessingGazeVector = 0x0080, - ProcessingExportTSV = 0x0100, - ProcessingExportC3D = 0x0200, - ProcessingExportMatlabFile = 0x0800, - ProcessingExportAviFile = 0x1000, - ProcessingPreProcess2D = 0x2000 - }; - - struct SPoint - { - float fX; - float fY; - float fZ; - }; - - struct SDiscoverResponse - { - char message[128]; - unsigned int nAddr; - unsigned short nBasePort; - }; - - - unsigned int GetSystemFrequency() const; -public: - struct SSettingsGeneralCamera - { - unsigned int nID; - ECameraModel eModel; - bool bUnderwater; - bool bSupportsHwSync; - unsigned int nSerial; - ECameraMode eMode; - EVideoResolution eVideoResolution; - EVideoAspectRatio eVideoAspectRatio; - unsigned int nVideoFrequency; - unsigned int nVideoExposure; // Micro seconds - unsigned int nVideoExposureMin; // Micro seconds - unsigned int nVideoExposureMax; // Micro seconds - unsigned int nVideoFlashTime; // Micro seconds - unsigned int nVideoFlashTimeMin; // Micro seconds - unsigned int nVideoFlashTimeMax; // Micro seconds - unsigned int nMarkerExposure; // Micro seconds - unsigned int nMarkerExposureMin; // Micro seconds - unsigned int nMarkerExposureMax; // Micro seconds - unsigned int nMarkerThreshold; - unsigned int nMarkerThresholdMin; - unsigned int nMarkerThresholdMax; - float fPositionX; - float fPositionY; - float fPositionZ; - float fPositionRotMatrix[3][3]; - unsigned int nOrientation; // Degrees - unsigned int nMarkerResolutionWidth; // Sub pixels - unsigned int nMarkerResolutionHeight; // Sub pixels - unsigned int nVideoResolutionWidth; // Pixels - unsigned int nVideoResolutionHeight; // Pixels - unsigned int nMarkerFOVLeft; // Pixels - unsigned int nMarkerFOVTop; // Pixels - unsigned int nMarkerFOVRight; // Pixels - unsigned int nMarkerFOVBottom; // Pixels - unsigned int nVideoFOVLeft; // Pixels - unsigned int nVideoFOVTop; // Pixels - unsigned int nVideoFOVRight; // Pixels - unsigned int nVideoFOVBottom; // Pixels - ESyncOutFreqMode eSyncOutMode[2]; - unsigned int nSyncOutValue[2]; - float fSyncOutDutyCycle[2]; // Percent - bool bSyncOutNegativePolarity[3]; - float fFocus; - float fAperture; - bool autoExposureEnabled; - float autoExposureCompensation; - int autoWhiteBalance; - }; - - struct SSettingsGeneralExternalTimebase - { - SSettingsGeneralExternalTimebase(): - bEnabled(false), - eSignalSource(SourceControlPort), - bSignalModePeriodic(false), - nFreqMultiplier(0), - nFreqDivisor(0), - nFreqTolerance(0), - fNominalFrequency(0.0f), - bNegativeEdge(false), - nSignalShutterDelay(0), - fNonPeriodicTimeout(0.0f) - { - } - - bool bEnabled; - ESignalSource eSignalSource; - bool bSignalModePeriodic; - unsigned int nFreqMultiplier; - unsigned int nFreqDivisor; - unsigned int nFreqTolerance; - float fNominalFrequency; - bool bNegativeEdge; - unsigned int nSignalShutterDelay; - float fNonPeriodicTimeout; - }; - - struct SSettingsGeneral - { - SSettingsGeneral(): - nCaptureFrequency(0), - fCaptureTime(0.0f), - bStartOnExternalTrigger(false), - bStartOnTrigNO(false), - bStartOnTrigNC(false), - bStartOnTrigSoftware(false), - eProcessingActions(EProcessingActions::ProcessingNone), - eRtProcessingActions(EProcessingActions::ProcessingNone), - eReprocessingActions(EProcessingActions::ProcessingNone) - { - sExternalTimebase = { }; - } - - unsigned int nCaptureFrequency; - float fCaptureTime; - bool bStartOnExternalTrigger; - bool bStartOnTrigNO; - bool bStartOnTrigNC; - bool bStartOnTrigSoftware; - SSettingsGeneralExternalTimebase sExternalTimebase; - EProcessingActions eProcessingActions; // Binary flags. - EProcessingActions eRtProcessingActions; // Binary flags. - EProcessingActions eReprocessingActions; // Binary flags. - std::vector< SSettingsGeneralCamera > vsCameras; - }; - - struct SSettings3DLabel - { - std::string oName; - unsigned int nRGBColor; - }; - - struct SSettingsBone - { - std::string fromName; - std::string toName; - unsigned int color; - }; - - struct SSettings3D - { - EAxis eAxisUpwards; - char pCalibrationTime[32]; - std::vector< SSettings3DLabel > s3DLabels; - std::vector< SSettingsBone > sBones; - }; - - struct SSettings6DOFBody - { - std::string oName; - unsigned int nRGBColor; - std::vector< SPoint > vsPoints; - }; - - struct SSettings6DOF - { - std::vector bodySettings; - std::string eulerFirst; - std::string eulerSecond; - std::string eulerThird; - }; - - struct SGazeVector - { - std::string name; - float frequency; - }; - - struct SAnalogDevice - { - unsigned int nDeviceID; - unsigned int nChannels; - std::string oName; - std::vector< std::string > voLabels; - unsigned int nFrequency; - std::string oUnit; - float fMinRange; - float fMaxRange; - std::vector< std::string > voUnits; - }; - - struct SForceChannel - { - unsigned int nChannelNumber; - float fConversionFactor; - }; - - struct SForcePlate - { - unsigned int nID; - unsigned int nAnalogDeviceID; - std::string oType; - std::string oName; - unsigned int nFrequency; - float fLength; - float fWidth; - SPoint asCorner[4]; - SPoint sOrigin; - std::vector< SForceChannel > vChannels; - bool bValidCalibrationMatrix; - float afCalibrationMatrix[12][12]; - unsigned int nCalibrationMatrixRows; - unsigned int nCalibrationMatrixColumns; - }; - - struct SSettingsForce - { - std::string oUnitLength; - std::string oUnitForce; - std::vector< SForcePlate > vsForcePlates; - }; - - struct SImageCamera - { - unsigned int nID; - bool bEnabled; - CRTPacket::EImageFormat eFormat; - unsigned int nWidth; - unsigned int nHeight; - float fCropLeft; - float fCropTop; - float fCropRight; - float fCropBottom; - }; - - struct SCalibrationFov - { - uint32_t left; - uint32_t top; - uint32_t right; - uint32_t bottom; - }; - - struct SCalibrationTransform - { - double x; - double y; - double z; - double r11; - double r12; - double r13; - double r21; - double r22; - double r23; - double r31; - double r32; - double r33; - }; - - struct SCalibrationIntrinsic - { - double focal_length; - double sensor_min_u; - double sensor_max_u; - double sensor_min_v; - double sensor_max_v; - double focal_length_u; - double focal_length_v; - double center_point_u; - double center_point_v; - double skew; - double radial_distortion_1; - double radial_distortion_2; - double radial_distortion_3; - double tangental_distortion_1; - double tangental_distortion_2; - }; - - struct SCalibrationCamera - { - bool active; - bool calibrated; - std::string message; - uint32_t point_count; - double avg_residual; - uint32_t serial; - std::string model; - uint32_t view_rotation; - SCalibrationFov fov_marker; - SCalibrationFov fov_marker_max; - SCalibrationFov fov_video; - SCalibrationFov fov_video_max; - SCalibrationTransform transform; - SCalibrationIntrinsic intrinsic; - }; - - struct SCalibration - { - bool calibrated = false; - std::string source = ""; - std::string created = ""; - std::string qtm_version = ""; - std::string type = ""; - double wand_length = 0.0; - uint32_t max_frames = 0; - double short_arm_end = 0.0; - double long_arm_end = 0.0; - double long_arm_middle = 0.0; - double result_std_dev = 0.0; - double result_min_max_diff = 0.0; - std::vector cameras; - }; - -public: - struct SSettingsSkeletonSegment : CRTPacket::SSkeletonSegment - { - std::string name; - int parentId; - int parentIndex; - }; - - struct SSettingsSkeleton - { - std::string name; - std::vector segments; - }; - - -public: - CRTProtocol(); - ~CRTProtocol(); - - bool Connect(const char* pServerAddr, unsigned short nPort, unsigned short* pnUDPServerPort = nullptr, - int nMajorVersion = MAJOR_VERSION, int nMinorVersion = MINOR_VERSION, bool bBigEndian = false); - unsigned short GetUdpServerPort(); - void Disconnect(); - bool Connected() const; - bool SetVersion(int nMajorVersion, int nMinorVersion); - bool GetVersion(unsigned int &nMajorVersion, unsigned int &nMinorVersion); - bool GetQTMVersion(char* pVersion, unsigned int nVersionLen); - bool GetByteOrder(bool &bBigEndian); - bool CheckLicense(const char* pLicenseCode); - bool DiscoverRTServer(unsigned short nServerPort, bool bNoLocalResponses, unsigned short nDiscoverPort = cDefaultAutoDescoverPort); - int GetNumberOfDiscoverResponses(); - bool GetDiscoverResponse(unsigned int nIndex, unsigned int &nAddr, unsigned short &nBasePort, std::string& message); - - bool GetCurrentFrame(const char* components); - bool GetCurrentFrame(unsigned int nComponentType, const SComponentOptions& componentOptions = { }); - bool StreamFrames(EStreamRate eRate, unsigned int nRateArg, unsigned short nUDPPort, const char* pUDPAddr, const char* components); - bool StreamFrames(EStreamRate eRate, unsigned int nRateArg, unsigned short nUDPPort, const char* pUDPAddr, - unsigned int nComponentType, const SComponentOptions& componentOptions = { }); - bool StreamFramesStop(); - bool GetState(CRTPacket::EEvent &eEvent, bool bUpdate = true, int nTimeout = cWaitForDataTimeout); - bool GetCapture(const char* pFileName, bool bC3D); - bool SendTrig(); - bool SetQTMEvent(const char* pLabel); - bool TakeControl(const char* pPassword = nullptr); - bool ReleaseControl(); - bool IsControlling(); - bool NewMeasurement(); - bool CloseMeasurement(); - bool StartCapture(); - bool StartRTOnFile(); - bool StopCapture(); - bool Calibrate(const bool refine, SCalibration &calibrationResult, int timeout = cWaitForCalibrationTimeout); - bool LoadCapture(const char* pFileName); - bool SaveCapture(const char* pFileName, bool bOverwrite, char* pNewFileName = nullptr, int nSizeOfNewFileName = 0); - bool LoadProject(const char* pFileName); - bool Reprocess(); - - static bool GetEventString(CRTPacket::EEvent eEvent, char* pStr); - static bool ConvertRateString(const char* pRate, EStreamRate &eRate, unsigned int &nRateArg); - static unsigned int ConvertComponentString(const char* pComponentType); - static bool GetComponentString(char* pComponentStr, unsigned int nComponentType, const SComponentOptions& options = SComponentOptions()); - static std::vector> GetComponents(const std::string componentsString); - static bool GetComponent(std::string componentStr, unsigned int &component, std::string &option); - - int ReceiveRTPacket(CRTPacket::EPacketType &eType, bool bSkipEvents = true, int nTimeout = cWaitForDataTimeout); // nTimeout < 0 : Blocking receive - - - CRTPacket* GetRTPacket(); - - bool ReadXmlBool(CMarkup* xml, const std::string& element, bool& value) const; - bool ReadCameraSystemSettings(); - bool ReadCalibrationSettings(); - bool Read3DSettings(bool &bDataAvailable); - bool Read6DOFSettings(bool &bDataAvailable); - bool ReadGazeVectorSettings(bool &bDataAvailable); - bool ReadAnalogSettings(bool &bDataAvailable); - bool ReadForceSettings(bool &bDataAvailable); - bool ReadImageSettings(bool &bDataAvailable); - bool ReadSkeletonSettings(bool &bDataAvailable, bool skeletonGlobalData = false); - - void GetSystemSettings( - unsigned int &nCaptureFrequency, float &fCaptureTime, - bool& bStartOnExtTrig, bool& trigNO, bool& trigNC, bool& trigSoftware, - EProcessingActions &eProcessingActions, EProcessingActions &eRtProcessingActions, EProcessingActions &eReprocessingActions) const; - - void GetCalibrationSettings(SCalibration &calibrationSettings) const; - - void GetExtTimeBaseSettings( - bool &bEnabled, ESignalSource &eSignalSource, - bool &bSignalModePeriodic, unsigned int &nFreqMultiplier, - unsigned int &nFreqDivisor, unsigned int &nFreqTolerance, - float &fNominalFrequency, bool &bNegativeEdge, - unsigned int &nSignalShutterDelay, float &fNonPeriodicTimeout) const; - - unsigned int GetCameraCount() const; - std::vector GetDevices() const; - - bool GetCameraSettings( - unsigned int nCameraIndex, unsigned int &nID, ECameraModel &eModel, - bool &bUnderwater, bool &bSupportsHwSync, unsigned int &nSerial, ECameraMode &eMode) const; - - bool GetCameraMarkerSettings( - unsigned int nCameraIndex, unsigned int &nCurrentExposure, - unsigned int &nMinExposure, unsigned int &nMaxExposure, - unsigned int &nCurrentThreshold, unsigned int &nMinThreshold, - unsigned int &nMaxThreshold) const; - - bool GetCameraVideoSettings( - unsigned int nCameraIndex, EVideoResolution &eVideoResolution, - EVideoAspectRatio &eVideoAspectRatio, unsigned int &nVideoFrequency, - unsigned int &nCurrentExposure, unsigned int &nMinExposure, - unsigned int &nMaxExposure, unsigned int &nCurrentFlashTime, - unsigned int &nMinFlashTime, unsigned int &nMaxFlashTime) const; - - bool GetCameraSyncOutSettings( - unsigned int nCameraIndex, unsigned int portNumber, ESyncOutFreqMode &eSyncOutMode, - unsigned int &nSyncOutValue, float &fSyncOutDutyCycle, - bool &bSyncOutNegativePolarity) const; - - bool GetCameraPosition( - unsigned int nCameraIndex, SPoint &sPoint, float fvRotationMatrix[3][3]) const; - - bool GetCameraOrientation( - unsigned int nCameraIndex, int &nOrientation) const; - - bool GetCameraResolution( - unsigned int nCameraIndex, unsigned int &nMarkerWidth, - unsigned int &nMarkerHeight, unsigned int &nVideoWidth, - unsigned int &nVideoHeight) const; - - bool GetCameraFOV( - unsigned int nCameraIndex, unsigned int &nMarkerLeft, unsigned int &nMarkerTop, - unsigned int &nMarkerRight, unsigned int &nMarkerBottom, - unsigned int &nVideoLeft, unsigned int &nVideoTop, - unsigned int &nVideoRight, unsigned int &nVideoBottom) const; - - bool GetCameraLensControlSettings(const unsigned int nCameraIndex, float* focus, float* aperture) const; - bool GetCameraAutoExposureSettings(const unsigned int nCameraIndex, bool* autoExposureEnabled, float* autoExposureCompensation) const; - bool GetCameraAutoWhiteBalance(const unsigned int nCameraIndex, bool* autoWhiteBalanceEnabled) const; - - EAxis Get3DUpwardAxis() const; - const char* Get3DCalibrated() const; - unsigned int Get3DLabeledMarkerCount() const; - const char* Get3DLabelName(unsigned int nMarkerIndex) const; - unsigned int Get3DLabelColor(unsigned int nMarkerIndex) const; - - unsigned int Get3DBoneCount() const; - const char* Get3DBoneFromName(unsigned int boneIndex) const; - const char* Get3DBoneToName(unsigned int boneIndex) const; - - void Get6DOFEulerNames(std::string &first, std::string &second, std::string &third) const; - unsigned int Get6DOFBodyCount() const; - const char* Get6DOFBodyName(unsigned int nBodyIndex) const; - unsigned int Get6DOFBodyColor(unsigned int nBodyIndex) const; - unsigned int Get6DOFBodyPointCount(unsigned int nBodyIndex) const; - bool Get6DOFBodyPoint(unsigned int nBodyIndex, unsigned int nMarkerIndex, SPoint &sPoint) const; - - unsigned int GetGazeVectorCount() const; - const char* GetGazeVectorName(unsigned int nGazeVectorIndex) const; - float GetGazeVectorFrequency(unsigned int nGazeVectorIndex) const; - - unsigned int GetAnalogDeviceCount() const; - bool GetAnalogDevice(unsigned int nDeviceIndex, unsigned int &nDeviceID, unsigned int &nChannels, - char* &pName, unsigned int &nFrequency, char* &pUnit, - float &fMinRange, float &fMaxRange) const; - const char* GetAnalogLabel(unsigned int nDeviceIndex, unsigned int nChannelIndex) const; - const char* GetAnalogUnit(unsigned int nDeviceIndex, unsigned int nChannelIndex) const; - - void GetForceUnits(char* &pLength, char* &pForce) const; - unsigned int GetForcePlateCount() const; - bool GetForcePlate(unsigned int nPlateIndex, unsigned int &nID, unsigned int &nAnalogDeviceID, - unsigned int &nFrequency, char* &pType, char* &pName, float &fLength, float &fWidth) const; - bool GetForcePlateLocation(unsigned int nPlateIndex, SPoint sCorner[4]) const; - bool GetForcePlateOrigin(unsigned int nPlateIndex, SPoint &sOrigin) const; - unsigned int GetForcePlateChannelCount(unsigned int nPlateIndex) const; - bool GetForcePlateChannel(unsigned int nPlateIndex, unsigned int nChannelIndex, - unsigned int &nChannelNumber, float &fConversionFactor) const; - bool GetForcePlateCalibrationMatrix(unsigned int nPlateIndex, float fvCalMatrix[12][12], unsigned int* rows, unsigned int* columns) const; - - unsigned int GetImageCameraCount() const; - bool GetImageCamera(unsigned int nCameraIndex, unsigned int &nCameraID, bool &bEnabled, - CRTPacket::EImageFormat &eFormat, unsigned int &nWidth, unsigned int &nHeight, - float &fCropLeft, float &fCropTop, float &fCropRight, float &fCropBottom) const; - - unsigned int GetSkeletonCount() const; - const char* GetSkeletonName(unsigned int skeletonIndex); - unsigned int GetSkeletonSegmentCount(unsigned int skeletonIndex); - bool GetSkeleton(unsigned int skeletonIndex, SSettingsSkeleton* skeleton); - bool GetSkeletonSegment(unsigned int skeletonIndex, unsigned int segmentIndex, SSettingsSkeletonSegment* segment);// parentIndex = -1 => No parent. - - - bool SetSystemSettings( - const unsigned int* pnCaptureFrequency, const float* pfCaptureTime, - const bool* pbStartOnExtTrig, const bool* trigNO, const bool* trigNC, const bool* trigSoftware, - const EProcessingActions* peProcessingActions, const EProcessingActions* peRtProcessingActions, const EProcessingActions* peReprocessingActions); - - bool SetExtTimeBaseSettings( - const bool* pbEnabled, const ESignalSource* peSignalSource, - const bool* pbSignalModePeriodic, const unsigned int* pnFreqMultiplier, - const unsigned int* pnFreqDivisor, const unsigned int* pnFreqTolerance, - const float* pfNominalFrequency, const bool* pbNegativeEdge, - const unsigned int* pnSignalShutterDelay, const float* pfNonPeriodicTimeout); - - bool SetCameraSettings( - const unsigned int nCameraID, const ECameraMode* peMode, - const float* pfMarkerExposure, const float* pfMarkerThreshold, - const int* pnOrientation); - - bool SetCameraVideoSettings( - const unsigned int nCameraID, const EVideoResolution* eVideoResolution, - const EVideoAspectRatio* eVideoAspectRatio, const unsigned int* pnVideoFrequency, - const float* pfVideoExposure, const float* pfVideoFlashTime); - - bool SetCameraSyncOutSettings( - const unsigned int nCameraID, const unsigned int portNumber, const ESyncOutFreqMode* peSyncOutMode, - const unsigned int* pnSyncOutValue, const float* pfSyncOutDutyCycle, - const bool* pbSyncOutNegativePolarity); - - bool SetCameraLensControlSettings(const unsigned int nCameraID, const float focus, const float aperture); - bool SetCameraAutoExposureSettings(const unsigned int nCameraID, const bool autoExposure, const float compensation); - bool SetCameraAutoWhiteBalance(const unsigned int nCameraID, const bool enable); - - bool SetImageSettings( - const unsigned int nCameraID, const bool* pbEnable, const CRTPacket::EImageFormat* peFormat, - const unsigned int* pnWidth, const unsigned int* pnHeight, const float* pfLeftCrop, - const float* pfTopCrop, const float* pfRightCrop, const float* pfBottomCrop); - - bool SetForceSettings( - const unsigned int nPlateID, const SPoint* psCorner1, const SPoint* psCorner2, - const SPoint* psCorner3, const SPoint* psCorner4); - - char* GetErrorString(); - -private: - bool SendString(const char* pCmdStr, int nType); - bool SendCommand(const char* pCmdStr); - bool SendCommand(const char* pCmdStr, char* pCommandResponseStr, unsigned int timeout = cWaitForDataTimeout); - bool SendXML(const char* pCmdStr); - void AddXMLElementBool(CMarkup* oXML, const char* tTag, const bool* pbValue, const char* tTrue = "True", const char* tFalse = "False"); - void AddXMLElementBool(CMarkup* oXML, const char* tTag, const bool bValue, const char* tTrue = "True", const char* tFalse = "False"); - void AddXMLElementInt(CMarkup* oXML, const char* tTag, const int* pnValue); - void AddXMLElementUnsignedInt(CMarkup* oXML, const char* tTag, const unsigned int* pnValue); - void AddXMLElementFloat(CMarkup* oXML, const char* tTag, const float* pfValue, unsigned int pnDecimals = 6); - bool CompareNoCase(std::string tStr1, const char* tStr2) const; - bool ReceiveCalibrationSettings(int timeout = cWaitForDataTimeout); - -private: - CNetwork* mpoNetwork; - CRTPacket* mpoRTPacket; - char* maDataBuff; - unsigned int mDataBuffSize; - CRTPacket::EEvent meLastEvent; - CRTPacket::EEvent meState; // Same as meLastEvent but without EventCameraSettingsChanged - int mnMinorVersion; - int mnMajorVersion; - bool mbBigEndian; - bool mbIsMaster; - SSettingsGeneral msGeneralSettings; - SSettings3D ms3DSettings; - SSettings6DOF mvs6DOFSettings; - std::vector mvsGazeVectorSettings; - std::vector mvsAnalogDeviceSettings; - SSettingsForce msForceSettings; - std::vector mvsImageSettings; - std::vector mSkeletonSettings; - SCalibration mCalibrationSettings; - char mSendBuffer[5000]; - char maErrorStr[1024]; - unsigned short mnBroadcastPort; - FILE* mpFileBuffer; - std::vector mvsDiscoverResponseList; -}; - - -#endif // RTPROTOCOL_H \ No newline at end of file diff --git a/navigation/qualisys_motion_capture/mocap_qualisys/launch/qualisys.launch b/navigation/qualisys_motion_capture/mocap_qualisys/launch/qualisys.launch deleted file mode 100644 index 17e85c18c..000000000 --- a/navigation/qualisys_motion_capture/mocap_qualisys/launch/qualisys.launch +++ /dev/null @@ -1,28 +0,0 @@ - - - - - - - - - - - - - - - - - - - [] - - - diff --git a/navigation/qualisys_motion_capture/mocap_qualisys/package.xml b/navigation/qualisys_motion_capture/mocap_qualisys/package.xml deleted file mode 100644 index ff47e19dc..000000000 --- a/navigation/qualisys_motion_capture/mocap_qualisys/package.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - mocap_qualisys - 0.0.1 - Driver for Qualisys - - Ke Sun - Apache-2.0 - Ke Sun - - catkin - geometry_msgs - roscpp - tf - tf_conversions - mocap_base - - - - diff --git a/navigation/qualisys_motion_capture/mocap_qualisys/src/QualisysDriver.cpp b/navigation/qualisys_motion_capture/mocap_qualisys/src/QualisysDriver.cpp deleted file mode 100644 index 871c840af..000000000 --- a/navigation/qualisys_motion_capture/mocap_qualisys/src/QualisysDriver.cpp +++ /dev/null @@ -1,288 +0,0 @@ -/* - * Copyright [2015] - * [Kartik Mohta ] - * [Ke Sun ] - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace Eigen; - -namespace mocap{ - -double QualisysDriver::deg2rad = M_PI / 180.0; - -bool QualisysDriver::init() { - // The base port (as entered in QTM, TCP/IP port number, in the RT output tab - // of the workspace options - nh.param("server_address", server_address, string("")); - nh.param("server_base_port", base_port, 22222); - nh.param("model_list", model_list, vector(0)); - int unsigned_frame_rate; - nh.param("frame_rate", unsigned_frame_rate, 0); - frame_rate = unsigned_frame_rate > 0 ? unsigned_frame_rate : 0; - nh.param("max_accel", max_accel, 10.0); - nh.param("publish_tf", publish_tf, false); - nh.param("fixed_frame_id", fixed_frame_id, string("mocap")); - int int_udp_port; - nh.param("udp_port", int_udp_port, -1); - nh.param("qtm_protocol_version", qtm_protocol_version, 18); - - if (server_address.empty()){ - ROS_FATAL("server_address parameter empty"); - return false; - } - - udp_stream_port = 0; - unsigned short* udp_port_ptr = nullptr; - if (int_udp_port >= 0 && int_udp_port < USHRT_MAX){ - udp_stream_port = static_cast(int_udp_port); - udp_port_ptr = &udp_stream_port; - } - else if (int_udp_port < -1 || int_udp_port > USHRT_MAX){ - ROS_WARN("Invalid UDP port %i, falling back to TCP", int_udp_port); - } - // Connecting to the server - ROS_INFO_STREAM("Connecting to QTM server at: " - << server_address << ":" << base_port); - // Major protocol version is always 1, so only the minor version can be set - const int major_protocol_version = 1; - int minor_protocol_version = qtm_protocol_version; - if (!port_protocol.Connect((char *)server_address.data(), - base_port, - udp_port_ptr, - major_protocol_version, - minor_protocol_version)) { - ROS_FATAL_STREAM("Connection to QTM server at: " - << server_address << ":" << base_port << " failed\n" - "Reason: " << port_protocol.GetErrorString()); - return false; - } - ROS_INFO_STREAM("Connected to " << server_address << ":" << base_port); - if (udp_stream_port > 0) - { - ROS_INFO("Streaming data to UDP port %i", udp_stream_port); - } - // Get 6DOF settings - bool bDataAvailable = false; - port_protocol.Read6DOFSettings(bDataAvailable); - if (bDataAvailable == false) { - ROS_FATAL_STREAM("Reading 6DOF body settings failed during intialization\n" - << "QTM error: " << port_protocol.GetErrorString()); - return false; - } - // Read system settings - if (!port_protocol.ReadCameraSystemSettings()){ - ROS_FATAL_STREAM("Failed to read system settings during intialization\n" - << "QTM error: " << port_protocol.GetErrorString()); - return false; - } - // Start streaming data frames - unsigned int system_frequency = port_protocol.GetSystemFrequency(); - CRTProtocol::EStreamRate stream_rate_mode = CRTProtocol::EStreamRate::RateAllFrames; - double dt = 1.0/(double)system_frequency; - if (frame_rate < system_frequency && frame_rate > 0){ - stream_rate_mode = CRTProtocol::EStreamRate::RateFrequency; - dt = 1.0/frame_rate; - } - else { - if (frame_rate > system_frequency){ - ROS_WARN("Requested capture rate %i larger than current system capture rate %i.", - frame_rate, system_frequency); - } - frame_rate = system_frequency; - } - bDataAvailable = port_protocol.StreamFrames( - stream_rate_mode, - frame_rate, // nRateArg - udp_stream_port, // nUDPPort - nullptr, // nUDPAddr - CRTProtocol::cComponent6d); - ROS_INFO("Frame rate: %i frames per second", frame_rate); - // Calculate covariance matrices - process_noise.topLeftCorner<6, 6>() = - 0.5*Matrix::Identity()*dt*dt*max_accel; - process_noise.bottomRightCorner<6, 6>() = - Matrix::Identity()*dt*max_accel; - process_noise *= process_noise; // Make it a covariance - measurement_noise = - Matrix::Identity()*1e-3; - measurement_noise *= measurement_noise; // Make it a covariance - model_set.insert(model_list.begin(), model_list.end()); - return true; -} - -void QualisysDriver::disconnect() { - ROS_INFO_STREAM("Disconnected from the QTM server at " - << server_address << ":" << base_port); - port_protocol.StreamFramesStop(); - port_protocol.Disconnect(); - return; -} - -bool QualisysDriver::run() { - prt_packet = port_protocol.GetRTPacket(); - CRTPacket::EPacketType e_type; - bool is_ok = false; - - if(port_protocol.ReceiveRTPacket(e_type, true)) { - switch(e_type) { - // Case 1 - sHeader.nType 0 indicates an error - case CRTPacket::PacketError: - ROS_ERROR_STREAM_THROTTLE( - 1, "Error when streaming frames: " - << port_protocol.GetRTPacket()->GetErrorString()); - break; - - // Case 2 - No more data - case CRTPacket::PacketNoMoreData: - ROS_ERROR_STREAM_THROTTLE(1, "No more data, check if RT capture is active"); - break; - - // Case 3 - Data received - case CRTPacket::PacketData: - handleFrame(); - is_ok = true; - break; - - //Case 9 - None type, sent on disconnet - case CRTPacket::PacketNone: - break; - - default: - ROS_WARN_THROTTLE(1, "Unhandled CRTPacket type, case: %i", e_type); - break; - } - } - else { // - ROS_ERROR_STREAM("QTM error when receiving packet:\n" << port_protocol.GetErrorString()); - } - return is_ok; -} - -void QualisysDriver::handleFrame() { - // Number of rigid bodies - int body_count = prt_packet->Get6DOFBodyCount(); - // Compute the timestamp - static double previous_frame_time = 0.0; - double current_frame_time = prt_packet->GetTimeStamp() / 1e6; - if(start_time_local_ == 0) - { - start_time_local_ = ros::Time::now().toSec(); - start_time_packet_ = current_frame_time; - previous_frame_time = current_frame_time; - } - else if (previous_frame_time > current_frame_time){ - ROS_WARN("Dropped old frame from time %fdropped, previous frame was from time %f", - current_frame_time, previous_frame_time); - } - else { - //ROS_INFO("Frame time drift: %f seconds", start_time_local_ + (current_frame_time - start_time_packet_) - ros::Time::now().toSec()); - previous_frame_time = current_frame_time; - } - for (int i = 0; i< body_count; ++i) { - string subject_name(port_protocol.Get6DOFBodyName(i)); - - // Process the subject if required - if (model_set.empty() || model_set.count(subject_name)) { - // Create a new subject if it does not exist - if (subjects.find(subject_name) == subjects.end()) { - subjects[subject_name] = Subject::SubjectPtr( - new Subject(&nh, subject_name, fixed_frame_id)); - subjects[subject_name]->setParameters( - process_noise, measurement_noise, frame_rate); - } - handleSubject(i); - } - } - return; -} - -void QualisysDriver::handleSubject(int sub_idx) { - // Name of the subject - string subject_name(port_protocol.Get6DOFBodyName(sub_idx)); - // Pose of the subject - const unsigned int matrix_size = 9; - float x, y, z; - float rot_array[matrix_size]; - prt_packet->Get6DOFBody(sub_idx, x, y, z, rot_array); - - // Check if the subject is tracked by looking for NaN in the received data - bool nan_in_matrix = false; - for (unsigned int i=0; i < matrix_size; i++){ - if (isnan(rot_array[i])) { - nan_in_matrix = true; - break; - } - } - if(isnan(x) || isnan(y) || isnan(z) || nan_in_matrix) { - if(subjects.at(subject_name)->getStatus() != Subject::LOST){ - subjects.at(subject_name)->disable(); - } - return; - } - - // Convert the rotation matrix to a quaternion - Eigen::Matrix rot_matrix(rot_array); - Eigen::Quaterniond m_att(rot_matrix.cast()); - // Check if the subject is beeing tracked - - // Convert mm to m - Eigen::Vector3d m_pos(x/1000.0, y/1000.0, z/1000.0); - // Re-enable the object if it is lost previously - if (subjects.at(subject_name)->getStatus() == Subject::LOST) { - subjects.at(subject_name)->enable(); - } - - const double packet_time = prt_packet->GetTimeStamp() / 1e6; - const double time = start_time_local_ + (packet_time - start_time_packet_); - - // Feed the new measurement to the subject - subjects.at(subject_name)->processNewMeasurement(time, m_att, m_pos); - - // Publish tf if requred - if (publish_tf && - subjects.at(subject_name)->getStatus() == Subject::TRACKED) { - - Quaterniond att = subjects.at(subject_name)->getAttitude(); - Vector3d pos = subjects.at(subject_name)->getPosition(); - tf::Quaternion att_tf; - tf::Vector3 pos_tf; - tf::quaternionEigenToTF(att, att_tf); - tf::vectorEigenToTF(pos, pos_tf); - ROS_DEBUG("Name: %s,\tindex: %i", subject_name.c_str(), sub_idx); - ROS_DEBUG("%s rot matrix:\n%f,\t%f,\t%f\n%f,\t%f,\t%f\n%f,\t%f,\t%f\n", - subject_name.c_str(), - rot_array[0], rot_array[1], rot_array[2], - rot_array[3], rot_array[4], rot_array[5], - rot_array[6], rot_array[7], rot_array[8]); - ROS_DEBUG("Position\nx: %f,\ty: %f\tz: %f", x/1000.0, y/1000.0, z/1000.0); - ROS_DEBUG("Quaternion rotation\nx: %f,\ty: %f,\tz: %f,\tw: %f,\t", - att_tf.getX(), att_tf.getY(), att_tf.getZ(), att_tf.getW()); - tf::StampedTransform stamped_transform = - tf::StampedTransform(tf::Transform(att_tf, pos_tf), - ros::Time(time), fixed_frame_id, subject_name); - tf_publisher.sendTransform(stamped_transform); - } - return; -} -} - diff --git a/navigation/qualisys_motion_capture/mocap_qualisys/src/qualisys.cpp b/navigation/qualisys_motion_capture/mocap_qualisys/src/qualisys.cpp deleted file mode 100644 index 728434645..000000000 --- a/navigation/qualisys_motion_capture/mocap_qualisys/src/qualisys.cpp +++ /dev/null @@ -1,50 +0,0 @@ -/* - * Copyright [2015] - * [Kartik Mohta ] - * [Ke Sun ] - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include -#include - -int main(int argc, char *argv[]) { - - ros::init(argc, argv, "qualisys"); - ros::NodeHandle nh("~"); - - mocap::QualisysDriver driver(nh); - if(!driver.init()) { - ROS_INFO("Initialization of the Qualisys driver failed!"); - return -1; - } - ROS_INFO("Successfully initialized QTM interface node!"); - - // ros::Rate r(200.0); - bool status = true; - while(ros::ok() && status == true) - { - //ROS_INFO("Runing"); - status = driver.run(); - //ROS_INFO("Spining"); - ros::spinOnce(); - //ROS_INFO("Sleeping"); - //ROS_INFO("Cycle time: %f", r.cycleTime().toSec()); - // r.sleep(); - } - ROS_INFO("QTM interface node shutting down"); - //driver.disconnect(); - - return 0; -} diff --git a/navigation/qualisys_motion_capture/mocap_vicon/CMakeLists.txt b/navigation/qualisys_motion_capture/mocap_vicon/CMakeLists.txt deleted file mode 100644 index 9d48e72db..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/CMakeLists.txt +++ /dev/null @@ -1,52 +0,0 @@ -cmake_minimum_required(VERSION 2.8.12) -project(mocap_vicon) - -set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/cmake") - -find_package(Boost REQUIRED COMPONENTS thread chrono) -find_package(Eigen3 REQUIRED) -find_package(catkin REQUIRED COMPONENTS - roscpp - geometry_msgs - tf - tf_conversions - mocap_base -) - -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall") - -catkin_package( - INCLUDE_DIRS include - LIBRARIES mocap_vicon_driver ViconDataStreamSDK_CPP - CATKIN_DEPENDS roscpp geometry_msgs tf tf_conversions mocap_base - DEPENDS Boost EIGEN3 -) - -add_subdirectory(src/vicon_sdk) - -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} -) - -add_library(mocap_vicon_driver - src/ViconDriver.cpp -) -target_link_libraries(mocap_vicon_driver - ViconDataStreamSDK_CPP - ${VICON_BOOST_LIBS} - ${catkin_LIBRARIES} -) -add_dependencies(mocap_vicon_driver - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) - -add_executable(mocap_vicon_node - src/vicon.cpp -) -target_link_libraries(mocap_vicon_node - mocap_vicon_driver - ${catkin_LIBRARIES} -) diff --git a/navigation/qualisys_motion_capture/mocap_vicon/include/mocap_vicon/ViconDriver.h b/navigation/qualisys_motion_capture/mocap_vicon/include/mocap_vicon/ViconDriver.h deleted file mode 100644 index 79cf861dd..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/include/mocap_vicon/ViconDriver.h +++ /dev/null @@ -1,112 +0,0 @@ -/* - * Copyright [2015] - * [Kartik Mohta ] - * [Ke Sun ] - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef VICON_DRIVER_H -#define VICON_DRIVER_H - -#include -#include -#include -#include -#include -#include -#include "ViconDataStreamSDK_CPP/DataStreamClient.h" // From Vicon's SDK - - -namespace mocap{ - -class ViconDriver: public MoCapDriverBase { - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - /* - * @brief Constructor - * @param nh Ros node - */ - ViconDriver(const ros::NodeHandle& n): - MoCapDriverBase (n), - client (new ViconDataStreamSDK::CPP::Client()), - max_accel (10.0), - frame_interval (0.01), - process_noise (Eigen::Matrix::Zero()), - measurement_noise (Eigen::Matrix::Zero()) { - return; - } - - /* - * @brief Destructor - */ - ~ViconDriver() { - disconnect(); - delete client; - return; - } - - /* - * @brief init Initialize the object - * @return True if successfully initialized - */ - bool init(); - - /* - * @brief run Start acquiring data from the server - */ - bool run(); - - /* - * @brief disconnect Disconnect to the server - * @Note The function is called automatically when the - * destructor is called. - */ - void disconnect(); - - private: - // Disable the copy constructor and assign operator - ViconDriver(const ViconDriver& ); - ViconDriver& operator=(const ViconDriver& ); - - // Handle a frame which contains the info of all subjects - void handleFrame(); - - // Handle a the info of a single subject - void handleSubject(int sub_idx); - - // Portal to communicate with the server - ViconDataStreamSDK::CPP::Client* client; - - // Max acceleration - double max_accel; - - // Average time interval between two frames - double frame_interval; - - // A set to hold the model names - std::set model_set; - - // Convariance matrices for initializing kalman filters - Eigen::Matrix process_noise; - Eigen::Matrix measurement_noise; - - // For multi-threading - boost::shared_mutex mtx; - -}; -} - - -#endif diff --git a/navigation/qualisys_motion_capture/mocap_vicon/launch/vicon.launch b/navigation/qualisys_motion_capture/mocap_vicon/launch/vicon.launch deleted file mode 100644 index 7e24a0ce1..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/launch/vicon.launch +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - [] - - - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/package.xml b/navigation/qualisys_motion_capture/mocap_vicon/package.xml deleted file mode 100644 index c34b95054..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - mocap_vicon - 0.0.1 - Driver for Vicon - - Ke Sun - Kartik Mohta - - Apache-2.0 - - Ke Sun - - catkin - - geometry_msgs - roscpp - tf - tf_conversions - mocap_base - - - - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/ViconDriver.cpp b/navigation/qualisys_motion_capture/mocap_vicon/src/ViconDriver.cpp deleted file mode 100644 index f245cf36a..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/ViconDriver.cpp +++ /dev/null @@ -1,216 +0,0 @@ -/* - * Copyright [2015] - * [Kartik Mohta ] - * [Ke Sun ] - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include -#include -#include -#include -#include - -using namespace std; -using namespace Eigen; -namespace ViconSDK = ViconDataStreamSDK::CPP; - -namespace mocap { - -bool ViconDriver::init() { - - nh.param("server_address", server_address, string("alkaline2")); - nh.param("model_list", model_list, vector(0)); - int signed_frame_rate; - nh.param("frame_rate", signed_frame_rate, 100); - frame_rate = signed_frame_rate > 0 ? signed_frame_rate : 0; - nh.param("max_accel", max_accel, 10.0); - nh.param("publish_tf", publish_tf, false); - nh.param("fixed_frame_id", fixed_frame_id, string("mocap")); - - frame_interval = 1.0 / static_cast(frame_rate); - double& dt = frame_interval; - process_noise.topLeftCorner<6, 6>() = - 0.5*Matrix::Identity()*dt*dt*max_accel; - process_noise.bottomRightCorner<6, 6>() = - Matrix::Identity()*dt*max_accel; - process_noise *= process_noise; // Make it a covariance - measurement_noise = - Matrix::Identity()*1e-3; - measurement_noise *= measurement_noise; // Make it a covariance - model_set.insert(model_list.begin(), model_list.end()); - - timespec ts_sleep; - ts_sleep.tv_sec = 0; - ts_sleep.tv_nsec = 100000000; - - // Connect to the server - ROS_INFO("Connecting to Vicon Datastream server at %s", server_address.c_str()); - bool is_connected = false; - for (int retry_cnt = 0; retry_cnt < 10; ++retry_cnt) { - client->Connect(server_address); - if(client->IsConnected().Connected) { - is_connected = true; - break; - } - else - nanosleep(&ts_sleep, NULL); - } - - // Report if cannot connect - if (!is_connected) { - ROS_WARN("Cannot Connect to Vicon server at %s", server_address.c_str()); - return false; - } - - // Configure the connection - ROS_INFO("Successfully Connect to Vicon server at %s", server_address.c_str()); - client->SetStreamMode(ViconSDK::StreamMode::ClientPull); - client->SetAxisMapping(ViconSDK::Direction::Forward, - ViconSDK::Direction::Left, ViconSDK::Direction::Up); - client->EnableSegmentData(); - if(!client->IsSegmentDataEnabled().Enabled) { - ROS_WARN("Segment data cannot be enabled."); - return false; - } - ROS_INFO("Successfully configure Vicon server at %s", server_address.c_str()); - - // Need to wait for some time after enabling data else you get junk frames - //struct timespec ts_sleep; - ts_sleep.tv_sec = 0; - ts_sleep.tv_nsec = 100000000; - nanosleep(&ts_sleep, NULL); - - return true; -} - - bool ViconDriver::run() { - ViconSDK::Result::Enum result = client->GetFrame().Result; - if (result != ViconSDK::Result::Success) - return false; - handleFrame(); - return true; -} - -void ViconDriver::disconnect() { - ROS_INFO_STREAM("Disconnected with the server at " - << server_address); - client->Disconnect(); - return; -} - -void ViconDriver::handleFrame() { - int body_count = client->GetSubjectCount().SubjectCount; - // Assign each subject with a thread - vector subject_threads; - subject_threads.reserve(body_count); - - for (int i = 0; i< body_count; ++i) { - string subject_name = - client->GetSubjectName(i).SubjectName; - - // Process the subject if required - if (model_set.empty() || model_set.count(subject_name)) { - // Create a new subject if it does not exist - if (subjects.find(subject_name) == subjects.end()) { - subjects[subject_name] = Subject::SubjectPtr( - new Subject(&nh, subject_name, fixed_frame_id)); - subjects[subject_name]->setParameters( - process_noise, measurement_noise, frame_rate); - } - // Handle the subject in a different thread - subject_threads.emplace_back(&ViconDriver::handleSubject, this, i); - //handleSubject(i); - } - } - - // Wait for all the threads to stop - for(auto &thread : subject_threads) { - thread.join(); - } - - // Send out warnings - for (auto it = subjects.begin(); - it != subjects.end(); ++it) { - Subject::Status status = it->second->getStatus(); - if (status == Subject::LOST) - ROS_WARN_THROTTLE(1, "Lose track of subject %s", (it->first).c_str()); - else if (status == Subject::INITIALIZING) - ROS_WARN("Initialize subject %s", (it->first).c_str()); - } - - return; -} - -void ViconDriver::handleSubject(int sub_idx) { - - boost::unique_lock write_lock(mtx); - // We assume each subject has only one segment - string subject_name = client->GetSubjectName(sub_idx).SubjectName; - string segment_name = client->GetSegmentName(subject_name, 0).SegmentName; - // Get the pose for the subject - ViconSDK::Output_GetSegmentGlobalTranslation trans = - client->GetSegmentGlobalTranslation(subject_name, segment_name); - ViconSDK::Output_GetSegmentGlobalRotationQuaternion quat = - client->GetSegmentGlobalRotationQuaternion(subject_name, segment_name); - write_lock.unlock(); - - //boost::shared_lock read_lock(mtx); - if(trans.Result != ViconSDK::Result::Success || - quat.Result != ViconSDK::Result::Success || - trans.Occluded || quat.Occluded) { - subjects[subject_name]->disable(); - return; - } - - // Convert the msgs to Eigen type - Eigen::Quaterniond m_att(quat.Rotation[3], - quat.Rotation[0], quat.Rotation[1], quat.Rotation[2]); - Eigen::Vector3d m_pos(trans.Translation[0]/1000, - trans.Translation[1]/1000, trans.Translation[2]/1000); - - // Re-enable the object if it is lost previously - if (subjects[subject_name]->getStatus() == Subject::LOST) { - subjects[subject_name]->enable(); - } - - // Feed the new measurement to the subject - double time = ros::Time::now().toSec(); - subjects[subject_name]->processNewMeasurement(time, m_att, m_pos); - //read_lock.unlock(); - - - // Publish tf if requred - if (publish_tf && - subjects[subject_name]->getStatus() == Subject::TRACKED) { - - Quaterniond att = subjects[subject_name]->getAttitude(); - Vector3d pos = subjects[subject_name]->getPosition(); - tf::Quaternion att_tf; - tf::Vector3 pos_tf; - tf::quaternionEigenToTF(att, att_tf); - tf::vectorEigenToTF(pos, pos_tf); - - tf::StampedTransform stamped_transform = - tf::StampedTransform(tf::Transform(att_tf, pos_tf), - ros::Time::now(), fixed_frame_id, subject_name); - write_lock.lock(); - tf_publisher.sendTransform(stamped_transform); - write_lock.unlock(); - } - - return; -} - -} diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon.cpp b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon.cpp deleted file mode 100644 index 69f90ab0b..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon.cpp +++ /dev/null @@ -1,44 +0,0 @@ -/* - * Copyright [2015] - * [Kartik Mohta ] - * [Ke Sun ] - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include -#include - -int main(int argc, char *argv[]) { - - ros::init(argc, argv, "vicon"); - ros::NodeHandle nh("~"); - - mocap::ViconDriver driver(nh); - if(!driver.init()) { - ROS_ERROR("Initialization of the Vicon driver failed"); - return -1; - } - ROS_INFO("Successfully initialize Vicon connection!"); - - while(ros::ok()) - { - driver.run(); - ros::spinOnce(); - } - - ROS_INFO("Shutting down"); - driver.disconnect(); - - return 0; -} diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/CMakeLists.txt b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/CMakeLists.txt deleted file mode 100644 index df4cc7b68..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/CMakeLists.txt +++ /dev/null @@ -1,16 +0,0 @@ -cmake_minimum_required(VERSION 2.8.12) - -add_library(ViconDataStreamSDK_CPP - Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/DataStreamClient.cpp - Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/CoreClient.cpp - Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/ClientUtils.cpp - Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/AxisMapping.cpp - Vicon/CrossMarket/DataStream/ViconCGStreamClientSDK/CGClient.cpp - Vicon/CrossMarket/DataStream/ViconCGStreamClient/ViconCGStreamClient.cpp - Vicon/CrossMarket/DataStream/ViconCGStreamClient/ViconCGStreamBayer.cpp - Vicon/CrossMarket/DataStream/ViconCGStreamClient/CGStreamReaderWriter.cpp -) -target_include_directories(ViconDataStreamSDK_CPP - PUBLIC Vicon/CrossMarket/DataStream - PRIVATE Vicon/CrossMarket) -target_link_libraries(ViconDataStreamSDK_CPP ${Boost_LIBRARIES}) diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/ApexHaptics.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/ApexHaptics.h deleted file mode 100644 index f4116367f..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/ApexHaptics.h +++ /dev/null @@ -1,77 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -#include "Item.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Used by the client to enable and disable haptics on Apex device. -class VApexHaptics : public VItem -{ -public: - - /// List of devices with haptics feedback on - std::set< unsigned int > m_OnDevicesList; - - - /// Equality operator - bool operator == ( const VApexHaptics & i_rOther ) const - { - return m_OnDevicesList == i_rOther.m_OnDevicesList; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::ApexHaptics; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return FILTER_NA; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_OnDevicesList ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_OnDevicesList ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/ApplicationInfo.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/ApplicationInfo.h deleted file mode 100644 index 37f886830..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/ApplicationInfo.h +++ /dev/null @@ -1,102 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VApplicationInfo class. - -#include "Item.h" - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains information about specific application settings that relate to the datastream -class VApplicationInfo : public VItem -{ -public: - - enum EAxisOrientation : ViconCGStreamType::Char - { - EZUp, - EYUp - }; - - /// Default construct this to have Z-up orientation, to maintain back compatibility with old servers - VApplicationInfo() : m_AxisOrientation( EZUp ) {} - - /// A transformation matrix representing the axis orientation of the application - EAxisOrientation m_AxisOrientation; - - /// Equality operator - bool operator == ( const VApplicationInfo& i_rOther ) const - { - return m_AxisOrientation == i_rOther.m_AxisOrientation; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::ApplicationInfo; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return FILTER_NA; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_AxisOrientation ) ; - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_AxisOrientation ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - -namespace ViconCGStreamIO -{ -//------------------------------------------------------------------------------------------------- - -template<> -struct VIsPod< ViconCGStream::VApplicationInfo::EAxisOrientation > -{ - enum - { - Answer = 1 - }; -}; - -//------------------------------------------------------------------------------------------------- -}; diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CameraCalibrationHealth.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CameraCalibrationHealth.h deleted file mode 100644 index b29f0a1da..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CameraCalibrationHealth.h +++ /dev/null @@ -1,92 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VCameraCalibrationHealth class. - -#include "Item.h" -#include "CameraCalibrationHealthDetail.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains a overall system Camera calibration health indicator as well as an individual indicator for each camera -/// Camera Health indicators vary from 0.0 (bad) to 1.0 (good) -class VCameraCalibrationHealth : public VItem -{ -public: - - /// Calibration Health for entire system - ViconCGStreamType::Double m_SystemHealth; - - /// Individual Calibration Health for cameras - std::vector< ViconCGStreamDetail::VCameraCalibrationHealth_Camera > m_Cameras; - - VCameraCalibrationHealth() - : m_SystemHealth( 0.0 ) - { - } - - /// Equality operator - bool operator == ( const VCameraCalibrationHealth & i_rOther ) const - { - return m_SystemHealth == i_rOther.m_SystemHealth && - m_Cameras == i_rOther.m_Cameras; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::CameraCalibrationHealth; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return FILTER_NA; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_SystemHealth ) && - i_rBuffer.Read( m_Cameras ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_SystemHealth ); - i_rBuffer.Write( m_Cameras ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CameraCalibrationHealthDetail.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CameraCalibrationHealthDetail.h deleted file mode 100644 index 9e8c0f966..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CameraCalibrationHealthDetail.h +++ /dev/null @@ -1,113 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the Detail of the ViconCGStream::VCameraCalirationHealth_Camera class - -#include "Enum.h" -#include - -#include - -namespace ViconCGStreamDetail -{ -//------------------------------------------------------------------------------------------------- - -/// Contains the Camera ID and Calibration Health for a single camera -class VCameraCalibrationHealth_Camera -{ -public: - - /// Device identifier - ViconCGStreamType::UInt32 m_DeviceID; - - /// Health Indicators - /// - /// All health indicators are computed over a variable time window - /// controlled by the underlying generator - /// - /// The underlying generator will currently use only LABELLED reconstructions - /// if available. Potentially model markers could be used. - /// - /// m_ReconUsage is the proportion of reconstructions visible in the - /// frustrum of the camera, that have associated centroids - /// varies between 0 and 1 (full usage) - ViconCGStreamType::Double m_ReconUsage; - - /// m_AverageReprojectionError - average distance between - /// centroid and projected reconstruction for this camera - ViconCGStreamType::Double m_AverageReprojectionError; - - /// m_NReconsInFrustrum - the number of reconstructions visible - /// in the frustrum of the camera - ViconCGStreamType::Double m_NReconsInFrustrum; - - /// m_NReprojections - the number of 2d-3d correspondences used to - /// to compute the m_AverageReprojectionError score - ViconCGStreamType::Double m_NReprojections; - - /// experimental - the average number of unused centroids per frame - ViconCGStreamType::Double m_UnusedCentroids; - - - /// Equality operator - bool operator == ( const VCameraCalibrationHealth_Camera & i_rOther ) const - { - return m_DeviceID == i_rOther.m_DeviceID - && m_ReconUsage == i_rOther.m_ReconUsage - && m_AverageReprojectionError == i_rOther.m_AverageReprojectionError - && m_NReconsInFrustrum == i_rOther.m_NReconsInFrustrum - && m_NReprojections == i_rOther.m_NReprojections - && m_UnusedCentroids == i_rOther.m_UnusedCentroids; - } - - /// Read function. - bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_DeviceID ) && - i_rBuffer.Read( m_ReconUsage ) && - i_rBuffer.Read( m_AverageReprojectionError ) && - i_rBuffer.Read( m_NReconsInFrustrum ) && - i_rBuffer.Read( m_NReprojections ) && - i_rBuffer.Read( m_UnusedCentroids ); - } - - /// Write function. - void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_DeviceID ); - i_rBuffer.Write( m_ReconUsage ); - i_rBuffer.Write( m_AverageReprojectionError ); - i_rBuffer.Write( m_NReconsInFrustrum ); - i_rBuffer.Write( m_NReprojections ); - i_rBuffer.Write( m_UnusedCentroids ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CameraCalibrationInfo.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CameraCalibrationInfo.h deleted file mode 100644 index 7084faa04..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CameraCalibrationInfo.h +++ /dev/null @@ -1,117 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -#include "Item.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -class VCameraCalibrationInfo : public VItem -{ -public: - - /// Camera identifier (device id) - ViconCGStreamType::UInt32 m_CameraID; - - /// Pose Translation - ViconCGStreamType::Double m_PoseTranslation[ 3 ]; - - /// Pose Rotation - ViconCGStreamType::Double m_PoseRotation[ 9 ]; - - /// Principal Point - ViconCGStreamType::Double m_PrincipalPoint[ 2 ]; - - /// Radial Distortion - ViconCGStreamType::Double m_RadialDistortion[ 2 ]; - - /// Focal Length - ViconCGStreamType::Double m_FocalLength; - - /// Skew - ViconCGStreamType::Double m_Skew; - - /// Image Error - ViconCGStreamType::Double m_ImageError; - - /// Equality operator - bool operator == ( const VCameraCalibrationInfo& i_rOther ) const - { - return m_CameraID == i_rOther.m_CameraID && - ViconCGStreamDetail::IsEqual( m_PoseTranslation, i_rOther.m_PoseTranslation ) && - ViconCGStreamDetail::IsEqual( m_PoseRotation, i_rOther.m_PoseRotation ) && - ViconCGStreamDetail::IsEqual( m_PrincipalPoint, i_rOther.m_PrincipalPoint ) && - ViconCGStreamDetail::IsEqual( m_RadialDistortion, i_rOther.m_RadialDistortion ) && - m_FocalLength == i_rOther.m_FocalLength && - m_Skew == i_rOther.m_Skew && - m_ImageError == i_rOther.m_ImageError; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::CameraCalibrationInfo; - } - - //// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return m_CameraID; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_CameraID ) && - i_rBuffer.Read( m_PoseTranslation ) && - i_rBuffer.Read( m_PoseRotation ) && - i_rBuffer.Read( m_PrincipalPoint ) && - i_rBuffer.Read( m_RadialDistortion ) && - i_rBuffer.Read( m_FocalLength ) && - i_rBuffer.Read( m_Skew ) && - i_rBuffer.Read( m_ImageError ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_CameraID ); - i_rBuffer.Write( m_PoseTranslation ); - i_rBuffer.Write( m_PoseRotation ); - i_rBuffer.Write( m_PrincipalPoint ); - i_rBuffer.Write( m_RadialDistortion ); - i_rBuffer.Write( m_FocalLength ); - i_rBuffer.Write( m_Skew ); - i_rBuffer.Write( m_ImageError ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CameraInfo.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CameraInfo.h deleted file mode 100644 index ea4411615..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CameraInfo.h +++ /dev/null @@ -1,154 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -#include "Item.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains the camera identifier, frame period, name, -class VCameraInfo : public VItem -{ -public: - - /// Camera identifier - ViconCGStreamType::UInt32 m_CameraID; - - /// Camera frame period. Defined as the numbers of ticks per frame on a 135Mhz clock. - ViconCGStreamType::UInt64 m_FramePeriod; - - /// Camera type - std::string m_Type; - - /// Camera display type - std::string m_DisplayType; - - /// Camera name - std::string m_Name; - - /// Resolution X - ViconCGStreamType::UInt32 m_ResolutionX; - - /// Resolution Y - ViconCGStreamType::UInt32 m_ResolutionY; - - /// Aspect ratio - ViconCGStreamType::Double m_AspectRatio; - - /// Minimum circularity - ViconCGStreamType::Double m_CircularityThreshold; - - /// Is the camera tracking centroids - bool m_bCentroidTracking; - - /// Is this a video camera - bool m_bIsVideoCamera; - - /// User friendly camera number. - ViconCGStreamType::UInt32 m_UserID; - - /// Marker velocity - ViconCGStreamType::Double m_MarkerVelocity; - - /// Equality function - bool IsEqual( const VCameraInfo & i_rOther ) const - { - return m_CameraID == i_rOther.m_CameraID && - m_FramePeriod == i_rOther.m_FramePeriod && - m_Type == i_rOther.m_Type && - m_DisplayType == i_rOther.m_DisplayType && - m_Name == i_rOther.m_Name && - m_ResolutionX == i_rOther.m_ResolutionX && - m_ResolutionY == i_rOther.m_ResolutionY && - m_AspectRatio == i_rOther.m_AspectRatio && - m_CircularityThreshold == i_rOther.m_CircularityThreshold && - m_bCentroidTracking == i_rOther.m_bCentroidTracking && - m_bIsVideoCamera == i_rOther.m_bIsVideoCamera && - m_UserID == i_rOther.m_UserID && - m_MarkerVelocity == i_rOther.m_MarkerVelocity; - } - - /// Equality operator - bool operator == ( const VCameraInfo & i_rOther ) const - { - return IsEqual( i_rOther ); - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::CameraInfo; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return m_CameraID; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_CameraID ) && - i_rBuffer.Read( m_FramePeriod ) && - i_rBuffer.Read( m_Type ) && - i_rBuffer.Read( m_DisplayType ) && - i_rBuffer.Read( m_Name ) && - i_rBuffer.Read( m_ResolutionX ) && - i_rBuffer.Read( m_ResolutionY ) && - i_rBuffer.Read( m_AspectRatio ) && - i_rBuffer.Read( m_CircularityThreshold ) && - i_rBuffer.Read( m_bCentroidTracking ) && - i_rBuffer.Read( m_bIsVideoCamera ) && - i_rBuffer.Read( m_UserID ) && - i_rBuffer.Read( m_MarkerVelocity ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_CameraID ); - i_rBuffer.Write( m_FramePeriod ); - i_rBuffer.Write( m_Type ); - i_rBuffer.Write( m_DisplayType ); - i_rBuffer.Write( m_Name ); - i_rBuffer.Write( m_ResolutionX ); - i_rBuffer.Write( m_ResolutionY ); - i_rBuffer.Write( m_AspectRatio ); - i_rBuffer.Write( m_CircularityThreshold ); - i_rBuffer.Write( m_bCentroidTracking ); - i_rBuffer.Write( m_bIsVideoCamera ); - i_rBuffer.Write( m_UserID ); - i_rBuffer.Write( m_MarkerVelocity ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CameraTimingInfo.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CameraTimingInfo.h deleted file mode 100644 index 6524009ff..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CameraTimingInfo.h +++ /dev/null @@ -1,94 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -#include "Item.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains the camera timing info. -class VCameraTimingInfo : public VItem -{ -public: - - /// Camera identifier - ViconCGStreamType::UInt32 m_CameraID; - - /// Camera frame period. Defined as the numbers of ticks per frame on a 135Mhz clock. - ViconCGStreamType::UInt64 m_FramePeriod; - - /// Camera frame offset. Tick offset in time of frame zero. - ViconCGStreamType::Int64 m_FrameOffset; - - /// Equality function - bool IsEqual( const VCameraTimingInfo & i_rOther ) const - { - return m_CameraID == i_rOther.m_CameraID && - m_FramePeriod == i_rOther.m_FramePeriod && - m_FrameOffset == i_rOther.m_FrameOffset; - } - - /// Equality operator - bool operator == ( const VCameraTimingInfo & i_rOther ) const - { - return IsEqual( i_rOther ); - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::CameraTimingInfo; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return m_CameraID; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_CameraID ) && - i_rBuffer.Read( m_FramePeriod ) && - i_rBuffer.Read( m_FrameOffset ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_CameraID ); - i_rBuffer.Write( m_FramePeriod ); - i_rBuffer.Write( m_FrameOffset ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CameraWand2d.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CameraWand2d.h deleted file mode 100644 index 546d74f33..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CameraWand2d.h +++ /dev/null @@ -1,104 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VCameraWand2d class. - -#include "Item.h" -#include "CameraWand2dDetail.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains a 2d wand captured in a camera during a wand wave. -class VCameraWand2d : public VItem -{ -public: - - /// Camera identifier - ViconCGStreamType::UInt32 m_CameraID; - - /// Wand session - ViconCGStreamType::UInt32 m_WaveSession; - - /// Wand points - std::vector< ViconCGStreamDetail::VCameraWand2d_Point > m_WandPoints; - - /// Cameras with wand - ViconCGStreamType::UInt32 m_CamerasWithWand; - - /// Moving - bool m_bMoving; - - /// Equality operator - bool operator == ( const VCameraWand2d & i_rOther ) const - { - return m_CameraID == i_rOther.m_CameraID && - m_WaveSession == i_rOther.m_WaveSession && - m_WandPoints == i_rOther.m_WandPoints && - m_CamerasWithWand == i_rOther.m_CamerasWithWand && - m_bMoving == i_rOther.m_bMoving; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::CameraWand2d; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return m_CameraID; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_CameraID ) && - i_rBuffer.Read( m_WaveSession ) && - i_rBuffer.Read( m_WandPoints ) && - i_rBuffer.Read( m_CamerasWithWand ) && - i_rBuffer.Read( m_bMoving ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_CameraID ); - i_rBuffer.Write( m_WaveSession ); - i_rBuffer.Write( m_WandPoints ); - i_rBuffer.Write( m_CamerasWithWand ); - i_rBuffer.Write( m_bMoving ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CameraWand2dDetail.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CameraWand2dDetail.h deleted file mode 100644 index 91e3da92e..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CameraWand2dDetail.h +++ /dev/null @@ -1,69 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the Detail of the ViconCGStream::VCameraWand2d class - -#include "Enum.h" -#include - - -namespace ViconCGStreamDetail -{ -//------------------------------------------------------------------------------------------------- - -/// Contains a wand point -class VCameraWand2d_Point -{ -public: - - // Position - ViconCGStreamType::Double m_Position[ 2 ]; - - /// Equality operator - bool operator == ( const VCameraWand2d_Point & i_rOther ) const - { - return m_Position[ 0 ] == i_rOther.m_Position[ 0 ] && - m_Position[ 1 ] == i_rOther.m_Position[ 1 ]; - } - - /// Read function. - bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_Position ); - } - - /// Write function. - void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_Position ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CameraWand3d.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CameraWand3d.h deleted file mode 100644 index d04a56940..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CameraWand3d.h +++ /dev/null @@ -1,98 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VCameraWand3d class. - -#include "Item.h" -#include "CameraWand3dDetail.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains a 3d wand captured in a camera during a wand wave. -class VCameraWand3d : public VItem -{ -public: - - /// Wand session - ViconCGStreamType::UInt32 m_WaveSession; - - /// Wand points - std::vector< ViconCGStreamDetail::VCameraWand3d_Point > m_WandPoints; - - /// Cameras with wand - ViconCGStreamType::UInt32 m_CamerasWithWand; - - /// Moving - bool m_bMoving; - - /// Equality operator - bool operator == ( const VCameraWand3d & i_rOther ) const - { - return m_WaveSession == i_rOther.m_WaveSession && - m_WandPoints == i_rOther.m_WandPoints && - m_CamerasWithWand == i_rOther.m_CamerasWithWand && - m_bMoving == i_rOther.m_bMoving; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::CameraWand3d; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return m_WaveSession; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_WaveSession ) && - i_rBuffer.Read( m_WandPoints ) && - i_rBuffer.Read( m_CamerasWithWand ) && - i_rBuffer.Read( m_bMoving ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_WaveSession ); - i_rBuffer.Write( m_WandPoints ); - i_rBuffer.Write( m_CamerasWithWand ); - i_rBuffer.Write( m_bMoving ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CameraWand3dDetail.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CameraWand3dDetail.h deleted file mode 100644 index 2fe64a03a..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CameraWand3dDetail.h +++ /dev/null @@ -1,69 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the Detail of the ViconCGStream::VCameraWand3d class - -#include "Enum.h" -#include - -namespace ViconCGStreamDetail -{ -//------------------------------------------------------------------------------------------------- - -/// Contains a wand point -class VCameraWand3d_Point -{ -public: - - // Position - ViconCGStreamType::Double m_Position[ 3 ]; - - /// Equality operator - bool operator == ( const VCameraWand3d_Point & i_rOther ) const - { - return m_Position[ 0 ] == i_rOther.m_Position[ 0 ] && - m_Position[ 1 ] == i_rOther.m_Position[ 1 ] && - m_Position[ 2 ] == i_rOther.m_Position[ 2 ]; - } - - /// Read function. - bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_Position ); - } - - /// Write function. - void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_Position ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CentreOfPressureFrame.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CentreOfPressureFrame.h deleted file mode 100644 index 7b1a3b80b..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CentreOfPressureFrame.h +++ /dev/null @@ -1,97 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VCentreOfPressureFrame class. - -#include "Item.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains a frame number, device and channel identifiers, and a frame of pressure centre samples in component order (measured in mm). -class VCentreOfPressureFrame : public VItem -{ -public: - - /// Device frame number - ViconCGStreamType::UInt32 m_FrameID; - - /// Device identifier - ViconCGStreamType::UInt32 m_DeviceID; - - /// Channel identifier - ViconCGStreamType::UInt32 m_ChannelID; - - /// Centre of pressure samples (in component order) - std::vector< ViconCGStreamType::Float > m_Samples; - - /// Equality operator - bool operator == ( const VCentreOfPressureFrame & i_rOther ) const - { - return m_FrameID == i_rOther.m_FrameID && - m_DeviceID == i_rOther.m_DeviceID && - m_ChannelID == i_rOther.m_ChannelID && - m_Samples == i_rOther.m_Samples; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::CentreOfPressureFrame; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return m_DeviceID; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_FrameID ) && - i_rBuffer.Read( m_DeviceID ) && - i_rBuffer.Read( m_ChannelID ) && - i_rBuffer.Read( m_Samples ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_FrameID ); - i_rBuffer.Write( m_DeviceID ); - i_rBuffer.Write( m_ChannelID ); - i_rBuffer.Write( m_Samples ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CentroidTracks.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CentroidTracks.h deleted file mode 100644 index 90811d155..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CentroidTracks.h +++ /dev/null @@ -1,92 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VCentroidTracks class. - -#include "Item.h" -#include "CentroidTracksDetail.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains a frame of umlabeled circles, along with the camera identifier and frame number. -class VCentroidTracks : public VItem -{ -public: - - /// Camera frame number - ViconCGStreamType::UInt32 m_FrameID; - - /// Camera identifier - ViconCGStreamType::UInt32 m_CameraID; - - /// Centroid tracks - std::vector< ViconCGStreamDetail::VCentroidTracks_CentroidTrack > m_CentroidTracks; - - /// Equality operator - bool operator == ( const VCentroidTracks & i_rOther ) const - { - return m_FrameID == i_rOther.m_FrameID && - m_CameraID == i_rOther.m_CameraID && - m_CentroidTracks == i_rOther.m_CentroidTracks; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::CentroidTracks; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return m_CameraID; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_FrameID ) && - i_rBuffer.Read( m_CameraID ) && - i_rBuffer.Read( m_CentroidTracks ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_FrameID ); - i_rBuffer.Write( m_CameraID ); - i_rBuffer.Write( m_CentroidTracks ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CentroidTracksDetail.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CentroidTracksDetail.h deleted file mode 100644 index e739af0f7..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CentroidTracksDetail.h +++ /dev/null @@ -1,84 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the details declaration of the ViconCGStream::VCentroidTracks class. - -#include "Enum.h" -#include - -namespace ViconCGStreamDetail -{ -//------------------------------------------------------------------------------------------------- - -/// Contains the centroid index, track id, track length and track quality. -class VCentroidTracks_CentroidTrack -{ -public: - - /// Centroid index - ViconCGStreamType::UInt32 m_CentroidIndex; - - /// 64 bit track id. - ViconCGStreamType::UInt64 m_TrackId; - - /// Length of track (saturates). - ViconCGStreamType::UInt32 m_TrackLength; - - /// Track quality. - ViconCGStreamType::Float m_TrackQuality; - - // Equality operator - bool operator == ( const VCentroidTracks_CentroidTrack & i_rOther ) const - { - return m_CentroidIndex == i_rOther.m_CentroidIndex && - m_TrackId == i_rOther.m_TrackId && - m_TrackLength == i_rOther.m_TrackLength && - m_TrackQuality == i_rOther.m_TrackQuality; - } - - /// Read function. - bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_CentroidIndex ) && - i_rBuffer.Read( m_TrackId ) && - i_rBuffer.Read( m_TrackLength ) && - i_rBuffer.Read( m_TrackQuality ); - } - - /// Write function. - void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_CentroidIndex ); - i_rBuffer.Write( m_TrackId ); - i_rBuffer.Write( m_TrackLength ); - i_rBuffer.Write( m_TrackQuality ); - } -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CentroidWeights.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CentroidWeights.h deleted file mode 100644 index c44a521ca..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CentroidWeights.h +++ /dev/null @@ -1,92 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VCentroids class. - -#include "Item.h" -#include "CentroidsDetail.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains a set of weights corresponding to unlabeled centroids, along with the camera identifier and frame number. -class VCentroidWeights : public VItem -{ -public: - - /// Camera frame number - ViconCGStreamType::UInt32 m_FrameID; - - /// Camera identifier - ViconCGStreamType::UInt32 m_CameraID; - - /// Centroids - std::vector< float > m_Weights; - - /// Equality operator - bool operator == ( const VCentroidWeights & i_rOther ) const - { - return m_FrameID == i_rOther.m_FrameID && - m_CameraID == i_rOther.m_CameraID && - m_Weights == i_rOther.m_Weights; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::CentroidWeights; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return m_CameraID; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_FrameID ) && - i_rBuffer.Read( m_CameraID ) && - i_rBuffer.Read( m_Weights ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_FrameID ); - i_rBuffer.Write( m_CameraID ); - i_rBuffer.Write( m_Weights ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/Centroids.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/Centroids.h deleted file mode 100644 index f35099def..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/Centroids.h +++ /dev/null @@ -1,92 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VCentroids class. - -#include "Item.h" -#include "CentroidsDetail.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains a frame of unlabeled centroids, along with the camera identifier and frame number. -class VCentroids : public VItem -{ -public: - - /// Camera frame number - ViconCGStreamType::UInt32 m_FrameID; - - /// Camera identifier - ViconCGStreamType::UInt32 m_CameraID; - - /// Centroids - std::vector< ViconCGStreamDetail::VCentroids_Centroid > m_Centroids; - - /// Equality operator - bool operator == ( const VCentroids & i_rOther ) const - { - return m_FrameID == i_rOther.m_FrameID && - m_CameraID == i_rOther.m_CameraID && - m_Centroids == i_rOther.m_Centroids; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::Centroids; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return m_CameraID; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_FrameID ) && - i_rBuffer.Read( m_CameraID ) && - i_rBuffer.Read( m_Centroids ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_FrameID ); - i_rBuffer.Write( m_CameraID ); - i_rBuffer.Write( m_Centroids ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CentroidsDetail.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CentroidsDetail.h deleted file mode 100644 index d066f38bd..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/CentroidsDetail.h +++ /dev/null @@ -1,78 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the details declaration of the ViconCGStream::VCentroids class. - -#include "Enum.h" -#include - -namespace ViconCGStreamDetail -{ -//------------------------------------------------------------------------------------------------- - -/// Contains the circle position, radius and accuracy -class VCentroids_Centroid -{ -public: - /// Circle position - ViconCGStreamType::Double m_Position[ 2 ]; - - /// Circle radius - ViconCGStreamType::Double m_Radius; - - /// Circle accuracy - ViconCGStreamType::Double m_Accuracy; - - // Equality operator - bool operator == ( const VCentroids_Centroid & i_rOther ) const - { - return m_Position[ 0 ] == i_rOther.m_Position[ 0 ] && - m_Position[ 1 ] == i_rOther.m_Position[ 1 ] && - m_Radius == i_rOther.m_Radius && - m_Accuracy == i_rOther.m_Accuracy; - } - - /// Read function. - bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_Position ) && - i_rBuffer.Read( m_Radius ) && - i_rBuffer.Read( m_Accuracy ); - } - - /// Write function. - void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_Position ); - i_rBuffer.Write( m_Radius ); - i_rBuffer.Write( m_Accuracy ); - } -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/ChannelInfo.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/ChannelInfo.h deleted file mode 100644 index 068497705..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/ChannelInfo.h +++ /dev/null @@ -1,179 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VChannelInfo class. - -#include "Item.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains the device and channel identifiers, the channel name, and the channel component names. -class VChannelInfo : public VItem -{ -public: - VChannelInfo() {} - VChannelInfo( ViconCGStreamType::UInt32 i_DeviceID, - ViconCGStreamType::UInt32 i_ChannelID, - std::string&& i_rName, - std::vector< std::string >&& i_rComponentNames ) - : m_DeviceID( i_DeviceID ) - , m_ChannelID( i_ChannelID ) - , m_Name( i_rName ) - , m_ComponentNames( i_rComponentNames ) - { - - } - - /// Device identifier - ViconCGStreamType::UInt32 m_DeviceID; - - /// Channel identifier - ViconCGStreamType::UInt32 m_ChannelID; - - /// Channel name - std::string m_Name; - - /// Component names - std::vector< std::string > m_ComponentNames; - - /// Equality function - bool IsEqual( const VChannelInfo & i_rOther ) const - { - return m_DeviceID == i_rOther.m_DeviceID && - m_ChannelID == i_rOther.m_ChannelID && - m_Name == i_rOther.m_Name && - m_ComponentNames == i_rOther.m_ComponentNames; - } - - /// Equality operator - bool operator == ( const VChannelInfo & i_rOther ) const - { - return IsEqual( i_rOther ); - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::ChannelInfo; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return m_DeviceID; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_DeviceID ) && - i_rBuffer.Read( m_ChannelID ) && - i_rBuffer.Read( m_Name ) && - i_rBuffer.Read( m_ComponentNames ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_DeviceID ); - i_rBuffer.Write( m_ChannelID ); - i_rBuffer.Write( m_Name ); - i_rBuffer.Write( m_ComponentNames ); - } -}; - -/// Extra information to supplement the VChannelInfo -/// Contains the device and channel identifiers, device unit and the type. -class VChannelInfoExtra : public VItem -{ -public: - - /// Device identifier - ViconCGStreamType::UInt32 m_DeviceID; - - /// Channel identifier - ViconCGStreamType::UInt32 m_ChannelID; - - /// Channel unit - std::string m_Unit; - - /// Channel type - std::string m_Type; - - /// Equality function - bool IsEqual( const VChannelInfoExtra & i_rOther ) const - { - return m_DeviceID == i_rOther.m_DeviceID && - m_ChannelID == i_rOther.m_ChannelID && - m_Unit == i_rOther.m_Unit && - m_Type == i_rOther.m_Type; - } - - /// Equality operator - bool operator == ( const VChannelInfoExtra & i_rOther ) const - { - return IsEqual( i_rOther ); - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::ChannelInfoExtra; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return m_DeviceID; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_DeviceID ) && - i_rBuffer.Read( m_ChannelID ) && - i_rBuffer.Read( m_Unit ) && - i_rBuffer.Read( m_Type ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_DeviceID ); - i_rBuffer.Write( m_ChannelID ); - i_rBuffer.Write( m_Unit ); - i_rBuffer.Write( m_Type ); - } -}; - - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/Contents.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/Contents.h deleted file mode 100644 index 1054971de..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/Contents.h +++ /dev/null @@ -1,89 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VContents class. - -#include "Item.h" -#include -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contents. -class VContents : public VItem -{ -public: - - /// Number of objects of each requested type present: - /// The map contains an entry for each object type requested, along - /// with a Size to indicate the number of objects present. - std::map< ViconCGStreamType::Enum, ViconCGStreamType::UInt32 > m_EnumsTable; - - /// Set of requested types that do not contain changes: - /// If all objects of a given type remain unchanged since the last object - /// packet, then the type value will appear in this container. - std::set< ViconCGStreamType::Enum > m_EnumsUnchanged; - - /// Equality operator - bool operator == ( const VContents & i_rOther ) const - { - return m_EnumsTable == i_rOther.m_EnumsTable && m_EnumsUnchanged == i_rOther.m_EnumsUnchanged; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::Contents; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return FILTER_NA; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_EnumsTable ) && - i_rBuffer.Read( m_EnumsUnchanged ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_EnumsTable ); - i_rBuffer.Write( m_EnumsUnchanged ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/DeviceInfo.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/DeviceInfo.h deleted file mode 100644 index 7d24513bf..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/DeviceInfo.h +++ /dev/null @@ -1,149 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VDeviceInfo class. - -#include "Item.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains the device identifier, frame period, and name of a newly connected or updated device. -class VDeviceInfo : public VItem -{ -public: - - /// Device identifier - ViconCGStreamType::UInt32 m_DeviceID; - - /// Device frame period. Defined as the numbers of ticks per frame on a 135Mhz clock. - ViconCGStreamType::UInt64 m_FramePeriod; - - /// Device name - std::string m_Name; - - /// Equality function - bool IsEqual( const VDeviceInfo & i_rOther ) const - { - return m_DeviceID == i_rOther.m_DeviceID && - m_FramePeriod == i_rOther.m_FramePeriod && - m_Name == i_rOther.m_Name; - } - - /// Equality operator - bool operator == ( const VDeviceInfo & i_rOther ) const - { - return IsEqual( i_rOther ); - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::DeviceInfo; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return m_DeviceID; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_DeviceID ) && - i_rBuffer.Read( m_FramePeriod ) && - i_rBuffer.Read( m_Name ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_DeviceID ); - i_rBuffer.Write( m_FramePeriod ); - i_rBuffer.Write( m_Name ); - } - -}; - -/// Contains the device identifier, frame start tick. -class VDeviceInfoExtra : public VItem -{ -public: - - /// Device identifier - ViconCGStreamType::UInt32 m_DeviceID; - - /// Device first device frame start tick offset from 0, on a 135Mhz clock. - ViconCGStreamType::UInt64 m_FrameStartTick; - - /// Equality function - bool IsEqual( const VDeviceInfoExtra & i_rOther ) const - { - return m_DeviceID == i_rOther.m_DeviceID && - m_FrameStartTick == i_rOther.m_FrameStartTick; - } - - /// Equality operator - bool operator == ( const VDeviceInfoExtra & i_rOther ) const - { - return IsEqual( i_rOther ); - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::DeviceInfoExtra; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return m_DeviceID; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_DeviceID ) && - i_rBuffer.Read( m_FrameStartTick ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_DeviceID ); - i_rBuffer.Write( m_FrameStartTick ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/Dummy.cpp b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/Dummy.cpp deleted file mode 100644 index e69de29bb..000000000 diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/EdgePairs.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/EdgePairs.h deleted file mode 100644 index 80f6cead1..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/EdgePairs.h +++ /dev/null @@ -1,91 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VEdgePairs class. - -#include "Item.h" -#include "EdgePairsDetail.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains a frame of edge pairs, along with the camera identifier and frame number. -class VEdgePairs : public VItem -{ -public: - - /// Camera frame number - ViconCGStreamType::UInt32 m_FrameID; - - /// Camera identifier - ViconCGStreamType::UInt32 m_CameraID; - - /// Edge pairs - std::vector< ViconCGStreamDetail::VEdgePairs_EdgePair > m_EdgePairs; - - bool operator == ( const VEdgePairs & i_rOther ) const - { - return m_FrameID == i_rOther.m_FrameID && - m_CameraID == i_rOther.m_CameraID && - m_EdgePairs == i_rOther.m_EdgePairs; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::EdgePairs; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return m_CameraID; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_FrameID ) && - i_rBuffer.Read( m_CameraID ) && - i_rBuffer.Read( m_EdgePairs ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_FrameID ); - i_rBuffer.Write( m_CameraID ); - i_rBuffer.Write( m_EdgePairs ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/EdgePairsDetail.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/EdgePairsDetail.h deleted file mode 100644 index de7140715..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/EdgePairsDetail.h +++ /dev/null @@ -1,73 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the detail declaration of the ViconCGStream::VEdgePairs class. - -#include "Enum.h" -#include -#include "IsEqual.h" - -namespace ViconCGStreamDetail -{ -//------------------------------------------------------------------------------------------------- - -/// Contains a frame of edge pairs, along with the camera identifier and frame number. -class VEdgePairs_EdgePair -{ -public: - - /// 2D position - ViconCGStreamType::Int16 m_Position[ 2 ]; - - /// Number of pixels in the line - ViconCGStreamType::UInt16 m_Length; - - /// Equality operator - bool operator == ( const VEdgePairs_EdgePair & i_rOther ) const - { - return IsEqual( m_Position, i_rOther.m_Position ) && - m_Length == i_rOther.m_Length; - } - - /// Read function. - bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_Position ) && - i_rBuffer.Read( m_Length ); - } - - /// Write function. - void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_Position ); - i_rBuffer.Write( m_Length ); - } -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/Enum.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/Enum.h deleted file mode 100644 index 314bface9..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/Enum.h +++ /dev/null @@ -1,199 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStreamEnum namespace. - -#include - -/** - * This namespace holds the enumerated values used to identify the various types of object found - * in the stream. Each packet consists of a header, followed by one or more object blocks. - * - * The packet header comprises of the \c PacketID tag (with the value of ViconCGStreamEnum::Objects), - * followed by the \c PacketLength value. Both the \c PacketID and \c PacketLength fields are - * ViconCGStreamType::UInt32 values. - * - * Each object (stored after the \c PacketLength field) has a block header, followed by the data - * specific to that object. The object header is similar to the packet header, in that it consists - * of an \c ObjectID, and the \c ObjectLength. The \c ObjectID value uniquely identifies the kind of - * object present. - */ -namespace ViconCGStreamEnum -{ -//------------------------------------------------------------------------------------------------- - -/// Explicitely invalid value (can be used for initialisation) -const ViconCGStreamType::Enum Invalid = -1; - -/// This value appears at the start of an object packet. -const ViconCGStreamType::Enum Objects = 0x1a2b0001; - -/// See ViconCGStream::VObjectEnums. -const ViconCGStreamType::Enum ObjectEnums = 100001; - -/// See ViconCGStream::VRequestFrame. -const ViconCGStreamType::Enum RequestFrame = 100002; - -/// See ViconCGStream::VKeepAlive. -const ViconCGStreamType::Enum KeepAlive = 100003; - -/// See ViconCGStream::VContents. -const ViconCGStreamType::Enum Contents = 100004; - -/// See ViconCGStream::VStartMulticastSender. -const ViconCGStreamType::Enum StartMulticastSender = 100005; - -/// See ViconCGStream::VStopMulticastSender. -const ViconCGStreamType::Enum StopMulticastSender = 100006; - -/// See ViconCGStream::VFilter. -const ViconCGStreamType::Enum Filter = 100007; - -/// See ViconCGStream::VStreamInfo. -const ViconCGStreamType::Enum StreamInfo = 1; - -/// See ViconCGStream::VSubjectInfo. -const ViconCGStreamType::Enum SubjectInfo = 2; - -/// See ViconCGStream::VSubjectHealth. -const ViconCGStreamType::Enum SubjectHealth = 29; - -/// See ViconCGStream::VDeviceInfo. -const ViconCGStreamType::Enum DeviceInfo = 3; - -/// See ViconCGStream::VChannelInfo. -const ViconCGStreamType::Enum ChannelInfo = 4; - -/// See ViconCGStream::VFrameInfo. -const ViconCGStreamType::Enum FrameInfo = 5; - -/// See ViconCGStream::VLabeledRecons. -const ViconCGStreamType::Enum LabeledRecons = 6; - -/// See ViconCGStream::VUnlabeledRecons. -const ViconCGStreamType::Enum UnlabeledRecons = 7; - -/// See ViconCGStream::VLocalSegments. -const ViconCGStreamType::Enum LocalSegments = 8; - -/// See ViconCGStream::VLocalSegments. -const ViconCGStreamType::Enum GlobalSegments = 9; - -/// See ViconCGStream::VForceFrame. -const ViconCGStreamType::Enum ForceFrame = 10; - -/// See ViconCGStream::VMomentFrame. -const ViconCGStreamType::Enum MomentFrame = 11; - -/// See ViconCGStream::VCentreOfPressureFrame. -const ViconCGStreamType::Enum CentreOfPressureFrame = 12; - -/// See ViconCGStream::VVoltageFrame. -const ViconCGStreamType::Enum VoltageFrame = 13; - -/// See ViconCGStream::VCameraInfo. -const ViconCGStreamType::Enum CameraInfo = 14; - -/// See ViconCGStream::VCameraInfo2. -const ViconCGStreamType::Enum CameraTimingInfo = 65001; - -/// See ViconCGStream::VCentroidTracks. -const ViconCGStreamType::Enum CentroidTracks = 15; - -/// See ViconCGStream::VCentroids. -const ViconCGStreamType::Enum Centroids = 16; - -/// See ViconCGStream::VGreyScaleBlobs. -const ViconCGStreamType::Enum GreyscaleBlobs = 17; - -/// See ViconCGStream::VEdgePairs. -const ViconCGStreamType::Enum EdgePairs = 18; - -/// See ViconCGStream:VCameraCalibrationInfo. -const ViconCGStreamType::Enum CameraCalibrationInfo = 19; - -/// See ViconCGStream::VForcePlateInfo. -const ViconCGStreamType::Enum ForcePlateInfo = 20; - -/// See ViconCGStream::VVideoFrame. -const ViconCGStreamType::Enum VideoFrame = 22; - -/// See ViconCGStream::VTimecode. -const ViconCGStreamType::Enum Timecode = 23; - -/// See ViconCGStream::VUnlabeledReconsRayAssignments -const ViconCGStreamType::Enum UnlabeledReconRayAssignments = 24; - -/// See ViconCGStream::VLabeledReconsRayAssignments -const ViconCGStreamType::Enum LabeledReconRayAssignments = 25; - -/// See ViconCGStream::VLatencyInfo -const ViconCGStreamType::Enum LatencyInfo = 26; - -/// See ViconCGStream::VCameraCalibrationHealth -const ViconCGStreamType::Enum CameraCalibrationHealth = 27; - -/// See ViconCGStream::VSubjectTopology -const ViconCGStreamType::Enum SubjectTopology = 28; - -/// See ViconCGStream:VCameraWand2d. -const ViconCGStreamType::Enum CameraWand2d = 30; - -/// See ViconCGStream:VCameraWand3d. -const ViconCGStreamType::Enum CameraWand3d = 31; - -/// See ViconCGStream::VEyeTrackerFrame. -const ViconCGStreamType::Enum EyeTrackerFrame = 32; - -/// See ViconCGStream::VEyeTrackerInfo. -const ViconCGStreamType::Enum EyeTrackerInfo = 33; - -/// See ViconCGStream::VChannelInfoExtra. -const ViconCGStreamType::Enum ChannelInfoExtra = 34; - -/// See ViconCGStream::VApexHaptics. -const ViconCGStreamType::Enum ApexHaptics = 35; - -/// See ViconCGStream::VHardwareFrameInfo. -const ViconCGStreamType::Enum HardwareFrameInfo = 36; - -/// See ViconCGStream::VApplicationInfo. -const ViconCGStreamType::Enum ApplicationInfo = 37; - -/// See ViconCGStream::VDeviceInfoExtra. -const ViconCGStreamType::Enum DeviceInfoExtra = 38; - -/// See ViconCGStream::VDeviceInfoExtra. -const ViconCGStreamType::Enum FrameRateInfo = 39; - -/// See ViconCGStream::VObjectQuality. -const ViconCGStreamType::Enum ObjectQuality = 40; - -/// See ViconCGStream::VObjectQuality. -const ViconCGStreamType::Enum CentroidWeights = 41; -//------------------------------------------------------------------------------------------------- -}; diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/EyeTrackerFrame.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/EyeTrackerFrame.h deleted file mode 100644 index 7e285f99d..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/EyeTrackerFrame.h +++ /dev/null @@ -1,93 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VEyeTrackerFrame class. - -#include "Item.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains a frame number, device identifiers and a frame of eye tracker samples. -class VEyeTrackerFrame : public VItem -{ -public: - - /// Device frame number - ViconCGStreamType::UInt32 m_FrameID; - - /// Device identifier - ViconCGStreamType::UInt32 m_DeviceID; - - /// Gaze direction as a unit vector. - /// In the eye coordinate system. - /// See: ViconCGStream::VEyeTrackerInfo. - ViconCGStreamType::Float m_GazeVector[ 3 ]; - - /// Equality operator - bool operator == ( const VEyeTrackerFrame & i_rOther ) const - { - return m_FrameID == i_rOther.m_FrameID && - m_DeviceID == i_rOther.m_DeviceID && - ViconCGStreamDetail::IsEqual( m_GazeVector, i_rOther.m_GazeVector ); - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::EyeTrackerFrame; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return m_DeviceID; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_FrameID ) && - i_rBuffer.Read( m_DeviceID ) && - i_rBuffer.Read( m_GazeVector ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_FrameID ); - i_rBuffer.Write( m_DeviceID ); - i_rBuffer.Write( m_GazeVector ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/EyeTrackerInfo.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/EyeTrackerInfo.h deleted file mode 100644 index 6a3b6471d..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/EyeTrackerInfo.h +++ /dev/null @@ -1,120 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VEyeTrackerInfo class. - -#include "Item.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains the device identifier, head segment information and local transformations -/// for the eye tracker. -/// Initially this is supporting the Ergoneers Dikablis device. - -class VEyeTrackerInfo : public VItem -{ -public: - - /// Device identifier - ViconCGStreamType::UInt32 m_DeviceID; - - /// Head subject identifier - /// The head subject is the one that has the head segment on it. - /// May be -1 if a segment is not set or is set incorrectly. - ViconCGStreamType::UInt32 m_SubjectID; - - /// Head segment identifier - /// The head segment is the one that has the eye tracker attached to it. - /// May be -1 if a segment is not set or is set incorrectly. - ViconCGStreamType::UInt32 m_SegmentID; - - /// Local translation from the head segment origin in mm. - /// This is the location of the centre of the eye-ball - /// with respect to the head. - ViconCGStreamType::Float m_LocalTranslation[ 3 ]; - - /// Local rotation matrix (row major format) - /// This is the rotation matrix that aligns the basic eye coordinate system. - /// The Z axis is forwards. - ViconCGStreamType::Float m_LocalRotation[ 9 ]; - - /// Equality function - bool IsEqual( const VEyeTrackerInfo & i_rOther ) const - { - return m_DeviceID == i_rOther.m_DeviceID && - m_SubjectID == i_rOther.m_SubjectID && - m_SegmentID == i_rOther.m_SegmentID && - ViconCGStreamDetail::IsEqual( m_LocalTranslation, i_rOther.m_LocalTranslation ) && - ViconCGStreamDetail::IsEqual( m_LocalRotation, i_rOther.m_LocalRotation ); - } - - /// Equality operator - bool operator == ( const VEyeTrackerInfo & i_rOther ) const - { - return IsEqual( i_rOther ); - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::EyeTrackerInfo; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return m_DeviceID; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_DeviceID ) && - i_rBuffer.Read( m_SubjectID ) && - i_rBuffer.Read( m_SegmentID ) && - i_rBuffer.Read( m_LocalTranslation ) && - i_rBuffer.Read( m_LocalRotation ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_DeviceID ); - i_rBuffer.Write( m_SubjectID ); - i_rBuffer.Write( m_SegmentID ); - i_rBuffer.Write( m_LocalTranslation ); - i_rBuffer.Write( m_LocalRotation ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/Filter.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/Filter.h deleted file mode 100644 index d4ebdf56d..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/Filter.h +++ /dev/null @@ -1,128 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Adds finer grained control to which items are sent from the CGServer. For example, -/// if Centroid data is requested, data for all cameras will be sent by default. Use a -/// filter to limit the data to a subset of cameras, and conserve network bandwidth and CPU load - -#include "Item.h" -#include -#include - -namespace ViconCGStream -{ - //------------------------------------------------------------------------------------------------- - - class VFilter : public VItem - { - public: - - // Maps Item type to a list of allowed instance IDs (i.e. CameraID, DeviceID, SubjectID, etc.) - typedef std::map< ViconCGStreamType::Enum, std::set< ViconCGStreamType::UInt32 > > TFilterMap; - - // Add a filter id that will pass the filter - void Add( ViconCGStreamType::Enum i_Type, ViconCGStreamType::UInt32 i_Filter ) - { - m_FilterMap[ i_Type ].insert( i_Filter ); - } - - // Remove a previously added filter - void Remove( ViconCGStreamType::Enum i_Type, ViconCGStreamType::UInt32 i_Filter ) - { - TFilterMap::iterator itr = m_FilterMap.find( i_Type ); - if ( itr != m_FilterMap.end() ) - { - itr->second.erase( i_Filter ); - } - } - - // Clear all filters for an item type - void Clear( ViconCGStreamType::Enum i_Type ) - { - TFilterMap::iterator itr = m_FilterMap.find( i_Type ); - if ( itr != m_FilterMap.end() ) - { - m_FilterMap.erase( itr ); - } - } - - // Reset the filter to empty - void ClearAll() - { - m_FilterMap.clear(); - } - - // Returns true if the item is allowed to be sent (meaning either the - // item filter ID is found, or no filter is set for the type id, allowing - // it by default ) - bool Allow( const VItem & i_rItem ) - { - TFilterMap::iterator iItems = m_FilterMap.find( i_rItem.TypeID() ); - if ( iItems != m_FilterMap.end() ) - { - std::set< ViconCGStreamType::UInt32 > & Allowed = iItems->second; - return std::find( Allowed.begin(), Allowed.end(), i_rItem.FilterID() ) != Allowed.end(); - } - - // not found, allow by default - return true; - } - - ///////////////////////////////////////////////////////////////////////////////////////////// - // VItem - - /// Type enum - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::Filter; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return FILTER_NA; - } - - /// Read function - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_FilterMap ); - } - - // Write function - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - return i_rBuffer.Write( m_FilterMap ); - } - - - private: - - TFilterMap m_FilterMap; - }; - -} diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/ForceFrame.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/ForceFrame.h deleted file mode 100644 index f8b29a106..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/ForceFrame.h +++ /dev/null @@ -1,97 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VForceFrame class. - -#include "Item.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains a frame number, device and channel identifiers, and a frame of force samples in component order (measured in N). -class VForceFrame : public VItem -{ -public: - - /// Device frame number - ViconCGStreamType::UInt32 m_FrameID; - - /// Device identifier - ViconCGStreamType::UInt32 m_DeviceID; - - /// Channel identifier - ViconCGStreamType::UInt32 m_ChannelID; - - /// Force samples (in component order) - std::vector< ViconCGStreamType::Float > m_Samples; - - /// Equality operator - bool operator == ( const VForceFrame & i_rOther ) const - { - return m_FrameID == i_rOther.m_FrameID && - m_DeviceID == i_rOther.m_DeviceID && - m_ChannelID == i_rOther.m_ChannelID && - m_Samples == i_rOther.m_Samples; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::ForceFrame; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return m_DeviceID; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_FrameID ) && - i_rBuffer.Read( m_DeviceID ) && - i_rBuffer.Read( m_ChannelID ) && - i_rBuffer.Read( m_Samples ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_FrameID ); - i_rBuffer.Write( m_DeviceID ); - i_rBuffer.Write( m_ChannelID ); - i_rBuffer.Write( m_Samples ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/ForcePlateInfo.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/ForcePlateInfo.h deleted file mode 100644 index 28cd3e46a..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/ForcePlateInfo.h +++ /dev/null @@ -1,116 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VForcePlateInfo class. - -#include "Item.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains the device identifier, world and local transformations, and bounds information. -/// Dimensions are in mm. -class VForcePlateInfo : public VItem -{ -public: - - /// Device identifier - ViconCGStreamType::UInt32 m_DeviceID; - - /// World translation - ViconCGStreamType::Float m_WorldTranslation[ 3 ]; - - /// World rotation matrix (row major format) - ViconCGStreamType::Float m_WorldRotation[ 9 ]; - - /// Device axis aligned bounds (min and max) - ViconCGStreamType::Float m_Bounds[ 6 ]; - - /// Local translation from the device origin in mm - ViconCGStreamType::Float m_LocalTranslation[ 3 ]; - - /// Local rotation matrix (row major format) - ViconCGStreamType::Float m_LocalRotation[ 9 ]; - - /// Equality function - bool IsEqual( const VForcePlateInfo & i_rOther ) const - { - return m_DeviceID == i_rOther.m_DeviceID && - ViconCGStreamDetail::IsEqual( m_WorldTranslation, i_rOther.m_WorldTranslation ) && - ViconCGStreamDetail::IsEqual( m_WorldRotation, i_rOther.m_WorldRotation ) && - ViconCGStreamDetail::IsEqual( m_Bounds, i_rOther.m_Bounds ) && - ViconCGStreamDetail::IsEqual( m_LocalTranslation, i_rOther.m_LocalTranslation ) && - ViconCGStreamDetail::IsEqual( m_LocalRotation, i_rOther.m_LocalRotation ); - } - - /// Equality operator - bool operator == ( const VForcePlateInfo & i_rOther ) const - { - return IsEqual( i_rOther ); - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::ForcePlateInfo; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return m_DeviceID; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_DeviceID ) && - i_rBuffer.Read( m_WorldTranslation ) && - i_rBuffer.Read( m_WorldRotation ) && - i_rBuffer.Read( m_Bounds ) && - i_rBuffer.Read( m_LocalTranslation ) && - i_rBuffer.Read( m_LocalRotation ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_DeviceID ); - i_rBuffer.Write( m_WorldTranslation ); - i_rBuffer.Write( m_WorldRotation ); - i_rBuffer.Write( m_Bounds ); - i_rBuffer.Write( m_LocalTranslation ); - i_rBuffer.Write( m_LocalRotation ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/FrameInfo.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/FrameInfo.h deleted file mode 100644 index b52d8b3de..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/FrameInfo.h +++ /dev/null @@ -1,78 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VFrameInfo class. - -#include "Item.h" - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains the master frame number for the current frame of objects. -class VFrameInfo : public VItem -{ -public: - - /// Stream frame number - ViconCGStreamType::UInt32 m_FrameID; - - /// Equality operator - bool operator == ( const VFrameInfo & i_rOther ) const - { - return m_FrameID == i_rOther.m_FrameID; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::FrameInfo; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return FILTER_NA; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_FrameID ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_FrameID ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/FrameRateInfo.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/FrameRateInfo.h deleted file mode 100644 index 02880e029..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/FrameRateInfo.h +++ /dev/null @@ -1,76 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VLatencyInfo class. - -#include "Item.h" - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains the latency samples taken -class VFrameRateInfo : public VItem -{ -public: - /// Samples - std::map< std::string, double > m_FrameRates; - - /// Equality operator - bool operator == ( const VFrameRateInfo& i_rOther ) const - { - return ( m_FrameRates == i_rOther.m_FrameRates ); - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::FrameRateInfo; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return FILTER_NA; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_FrameRates ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_FrameRates ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/GlobalSegments.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/GlobalSegments.h deleted file mode 100644 index 1020ca931..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/GlobalSegments.h +++ /dev/null @@ -1,85 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VGlobalSegments class. - -#include "Item.h" -#include "GlobalSegmentsDetail.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains a vector of segment global poses for a single subject. -class VGlobalSegments : public VItem -{ -public: - - /// Subject identifier - ViconCGStreamType::UInt32 m_SubjectID; - - /// Global segments - std::vector< ViconCGStreamDetail::VGlobalSegments_Segment > m_Segments; - - // Equality operator - bool operator == ( const VGlobalSegments & i_rOther ) const - { - return m_Segments == i_rOther.m_Segments; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::GlobalSegments; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return m_SubjectID; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_SubjectID ) && - i_rBuffer.Read( m_Segments ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_SubjectID ); - i_rBuffer.Write( m_Segments ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/GlobalSegmentsDetail.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/GlobalSegmentsDetail.h deleted file mode 100644 index 0639fb0f4..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/GlobalSegmentsDetail.h +++ /dev/null @@ -1,77 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the detail declaration of the ViconCGStream::VGlobalSegments class. - -#include "Enum.h" -#include -#include "IsEqual.h" - -namespace ViconCGStreamDetail -{ -//------------------------------------------------------------------------------------------------- - -class VGlobalSegments_Segment -{ -public: - /// Segment identifier - ViconCGStreamType::UInt32 m_SegmentID; - - /// Segment translation from the world origin - ViconCGStreamType::Double m_Translation[ 3 ]; - - /// Segment rotation matrix (row major format) - ViconCGStreamType::Double m_Rotation[ 9 ]; - - // Equality operator - bool operator == ( const VGlobalSegments_Segment & i_rOther ) const - { - return m_SegmentID == i_rOther.m_SegmentID && - ViconCGStreamDetail::IsEqual( m_Translation, i_rOther.m_Translation ) && - ViconCGStreamDetail::IsEqual( m_Rotation, i_rOther.m_Rotation ); - } - - /// Read function. - bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_SegmentID ) && - i_rBuffer.Read( m_Translation ) && - i_rBuffer.Read( m_Rotation ); - } - - /// Write function. - void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_SegmentID ); - i_rBuffer.Write( m_Translation ); - i_rBuffer.Write( m_Rotation ); - } -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/GreyscaleBlobs.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/GreyscaleBlobs.h deleted file mode 100644 index ade113815..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/GreyscaleBlobs.h +++ /dev/null @@ -1,91 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VGreyscaleBlobs class. - -#include "GreyscaleBlobsDetail.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains a frame of greyscale blobs, along with the camera identifier and frame number. -class VGreyscaleBlobs : public VItem -{ -public: - - /// Camera frame number - ViconCGStreamType::UInt32 m_FrameID; - - /// Camera identifier - ViconCGStreamType::UInt32 m_CameraID; - - /// Greyscale blobs - std::vector< ViconCGStreamDetail::VGreyscaleBlobs_GreyscaleBlob > m_GreyscaleBlobs; - - /// Equality operator - bool operator == ( const VGreyscaleBlobs & i_rOther ) const - { - return m_FrameID == i_rOther.m_FrameID && - m_CameraID == i_rOther.m_CameraID && - m_GreyscaleBlobs == i_rOther.m_GreyscaleBlobs; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::GreyscaleBlobs; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return m_CameraID; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_FrameID ) && - i_rBuffer.Read( m_CameraID ) && - i_rBuffer.Read( m_GreyscaleBlobs ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_FrameID ); - i_rBuffer.Write( m_CameraID ); - i_rBuffer.Write( m_GreyscaleBlobs ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/GreyscaleBlobsDetail.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/GreyscaleBlobsDetail.h deleted file mode 100644 index 3aec25169..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/GreyscaleBlobsDetail.h +++ /dev/null @@ -1,105 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the detail declaration of the ViconCGStream::VGreyscaleBlobs class. - -#include "Enum.h" -#include -#include - -namespace ViconCGStreamDetail -{ -//------------------------------------------------------------------------------------------------- - -/** - * Contains a greyscale line - */ -class VGreyscaleBlobs_GreyscaleLine -{ -public: - - /// Greyscale values for the line pixels - std::vector< ViconCGStreamType::UInt8 > m_Greyscale; - - /// 2D position - ViconCGStreamType::Int16 m_Position[ 2 ]; - - /// Equality operator - bool operator == ( const VGreyscaleBlobs_GreyscaleLine & i_rOther ) const - { - return m_Position[ 0 ] == i_rOther.m_Position[ 0 ] && - m_Position[ 1 ] == i_rOther.m_Position[ 1 ] && - m_Greyscale == i_rOther.m_Greyscale; - } - - /// Read function. - bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_Greyscale ) && - i_rBuffer.Read( m_Position ); - } - - /// Write function. - void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_Greyscale ); - i_rBuffer.Write( m_Position ); - } -}; - -/** - * Contains greyscale lines - */ -class VGreyscaleBlobs_GreyscaleBlob -{ -public: - - /// Vector of lines comprising the blob - std::vector< VGreyscaleBlobs_GreyscaleLine > m_GreyscaleLines; - - /// Equality operator - bool operator == ( const VGreyscaleBlobs_GreyscaleBlob & i_rOther ) const - { - return m_GreyscaleLines == i_rOther.m_GreyscaleLines; - } - - /// Read function. - bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_GreyscaleLines ); - } - - /// Write function. - void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_GreyscaleLines ); - } -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/HardwareFrameInfo.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/HardwareFrameInfo.h deleted file mode 100644 index 746fe7fbb..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/HardwareFrameInfo.h +++ /dev/null @@ -1,79 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VHardwareFrameInfo class. - -#include "Item.h" - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains the native hardware frame number for the current frame. -/// This is typically for debugging purposes - most users should use VFrameInfo. -class VHardwareFrameInfo : public VItem -{ -public: - - /// Stream frame number - ViconCGStreamType::UInt32 m_HardwareFrame; - - /// Equality operator - bool operator == ( const VHardwareFrameInfo & i_rOther ) const - { - return m_HardwareFrame == i_rOther.m_HardwareFrame; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::HardwareFrameInfo; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return FILTER_NA; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_HardwareFrame ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_HardwareFrame ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/IsEqual.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/IsEqual.h deleted file mode 100644 index 8406b2ac6..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/IsEqual.h +++ /dev/null @@ -1,58 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -namespace ViconCGStreamDetail -{ -//------------------------------------------------------------------------------------------------- - -template< unsigned int N > -struct VDetail -{ - template< typename T > - static bool IsEqual( const T * i_pLeft, const T * i_pRight ) - { - return *i_pLeft == *i_pRight && VDetail< N - 1 >::IsEqual( i_pLeft + 1, i_pRight + 1 ); - } -}; - -template<> -struct VDetail< 1 > -{ - template< typename T > - static bool IsEqual( const T * i_pLeft, const T * i_pRight ) - { - return *i_pLeft == *i_pRight; - } -}; - -template< typename T, unsigned int N > -bool IsEqual( const T ( & i_rpLeft )[ N ], const T ( & i_rpRight )[ N ] ) -{ - return VDetail< N >::IsEqual( i_rpLeft, i_rpRight ); -} - -//------------------------------------------------------------------------------------------------- -}; diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/Item.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/Item.h deleted file mode 100644 index e9364f4cc..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/Item.h +++ /dev/null @@ -1,63 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -#include "Enum.h" -#include -#include "IsEqual.h" - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -const ViconCGStreamType::Int32 FILTER_NA = -1; - -/// This is the base class for all top level items -class VItem -{ -public: - - /// Type enum - virtual ViconCGStreamType::Enum TypeID() const = 0; - - /// The value used to filter items belonging to a logical group - /// Examples of return values are CameraID, DeviceID, SubjectID, etc. - /// Top-level items that have no affinity with logical children should return FILTER_NA - virtual ViconCGStreamType::UInt32 FilterID() const = 0; - - /// Read function - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) = 0; - - // Write function - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const = 0; - - // Virtual destructor - virtual ~VItem() - { - } -}; - -//------------------------------------------------------------------------------------------------- -}; diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/KeepAlive.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/KeepAlive.h deleted file mode 100644 index e30b9fe01..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/KeepAlive.h +++ /dev/null @@ -1,74 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VKeepAlive class. - -#include "Item.h" - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Keep alive object. -class VKeepAlive : public VItem -{ -public: - - /// Equality operator. - bool operator == ( const VKeepAlive & i_rKeepAlive ) const - { - return true; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::KeepAlive; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return FILTER_NA; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return true; - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/LabeledReconRayAssignments.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/LabeledReconRayAssignments.h deleted file mode 100644 index 6baa2d138..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/LabeledReconRayAssignments.h +++ /dev/null @@ -1,79 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VLabeledReconRayAssignments class. - -#include "Item.h" -#include "ReconRayAssignmentsDetail.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Labeled ray assignments -class VLabeledReconRayAssignments : public VItem -{ -public: - /// Recon ray assignments. - std::vector< ViconCGStreamDetail::VReconRayAssignments > m_ReconRayAssignments; - - /// Equality operator. - bool operator == ( const VLabeledReconRayAssignments & i_rOther ) const - { - return m_ReconRayAssignments == i_rOther.m_ReconRayAssignments; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const { - return ViconCGStreamEnum::LabeledReconRayAssignments; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return FILTER_NA; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_ReconRayAssignments ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_ReconRayAssignments ); - } - -}; - -//------------------------------------------------------------------------------------------------- -} - - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/LabeledRecons.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/LabeledRecons.h deleted file mode 100644 index a979b5ace..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/LabeledRecons.h +++ /dev/null @@ -1,80 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VLabeledRecons class. - -#include "Item.h" -#include "LabeledReconsDetail.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains a frame of labeled recons, consisting of subject and marker identifiers, radii, positions and covariances. -class VLabeledRecons : public VItem -{ -public: - - /// Labeled recons - std::vector< ViconCGStreamDetail::VLabeledRecons_LabeledRecon > m_LabeledRecons; - - /// Equality operator - bool operator == ( const VLabeledRecons & i_rOther ) const - { - return m_LabeledRecons == i_rOther.m_LabeledRecons; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::LabeledRecons; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return FILTER_NA; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_LabeledRecons ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_LabeledRecons ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/LabeledReconsDetail.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/LabeledReconsDetail.h deleted file mode 100644 index dc984ec40..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/LabeledReconsDetail.h +++ /dev/null @@ -1,100 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the detail declaration of the ViconCGStream::VLabeledRecons class. - -#include "Enum.h" -#include -#include "IsEqual.h" - -namespace ViconCGStreamDetail -{ -//------------------------------------------------------------------------------------------------- - -/** - * Contains a single labeled recon. - */ -class VLabeledRecons_LabeledRecon -{ -public: - - /// Subject identifier - ViconCGStreamType::UInt32 m_SubjectID; - - /// Marker identifier (for subject) - ViconCGStreamType::UInt32 m_MarkerID; - - /// Recon radius - ViconCGStreamType::Double m_Radius; - - /// Recon position - ViconCGStreamType::Double m_Position[ 3 ]; - - /// Recon covariance matrix (row major format) - ViconCGStreamType::Double m_Covariance[ 9 ]; - - /// Trajectory id - ViconCGStreamType::UInt32 m_TrajectoryId; - - /// Equality operator - bool operator == ( const VLabeledRecons_LabeledRecon & i_rOther ) const - { - return m_SubjectID == i_rOther.m_SubjectID && - m_MarkerID == i_rOther.m_MarkerID && - m_Radius == i_rOther.m_Radius && - IsEqual( m_Position, i_rOther.m_Position ) && - IsEqual( m_Covariance, i_rOther.m_Covariance ) && - m_TrajectoryId == i_rOther.m_TrajectoryId; - } - - /// Read function. - bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_SubjectID ) && - i_rBuffer.Read( m_MarkerID ) && - i_rBuffer.Read( m_Radius ) && - i_rBuffer.Read( m_Position ) && - i_rBuffer.Read( m_Covariance ) && - i_rBuffer.Read( m_TrajectoryId ); - } - - /// Write function. - void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_SubjectID ); - i_rBuffer.Write( m_MarkerID ); - i_rBuffer.Write( m_Radius ); - i_rBuffer.Write( m_Position ); - i_rBuffer.Write( m_Covariance ); - i_rBuffer.Write( m_TrajectoryId ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/LatencyInfo.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/LatencyInfo.h deleted file mode 100644 index ef622766e..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/LatencyInfo.h +++ /dev/null @@ -1,78 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VLatencyInfo class. - -#include "Item.h" -#include "LatencyInfoDetail.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains the latency samples taken -class VLatencyInfo : public VItem -{ -public: - /// Samples - std::vector< ViconCGStreamDetail::VLatencyInfo_Sample > m_Samples; - - /// Equality operator - bool operator == ( const VLatencyInfo& i_rOther ) const - { - return ( m_Samples == i_rOther.m_Samples ); - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::LatencyInfo; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return FILTER_NA; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_Samples ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_Samples ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/LatencyInfoDetail.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/LatencyInfoDetail.h deleted file mode 100644 index 79c2203f5..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/LatencyInfoDetail.h +++ /dev/null @@ -1,78 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the details declaration of the ViconCGStream::VLatencyInfo class. - -#include "Enum.h" -#include -#include "IsEqual.h" - -namespace ViconCGStreamDetail -{ -//------------------------------------------------------------------------------------------------- - -/// Contains a string and a duration in seconds since the previous sample was taken -class VLatencyInfo_Sample -{ -public: - /// Sample name - std::string m_Name; - - /// Duration in seconds since the previous sample was taken - ViconCGStreamType::Double m_Latency; - - /// Equality function - bool IsEqual( const VLatencyInfo_Sample & i_rOther ) const - { - return m_Name == i_rOther.m_Name && - m_Latency == i_rOther.m_Latency; - } - - /// Equality operator - bool operator == ( const VLatencyInfo_Sample & i_rOther ) const - { - return IsEqual( i_rOther ); - } - - /// Read function. - bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_Name ) && - i_rBuffer.Read( m_Latency ); - } - - /// Write function. - void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_Name ); - i_rBuffer.Write( m_Latency ); - } -}; - -//------------------------------------------------------------------------------------------------- - -}; diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/LocalSegments.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/LocalSegments.h deleted file mode 100644 index 753c097c7..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/LocalSegments.h +++ /dev/null @@ -1,94 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VLocalSegments class. - -#include "Item.h" -#include "LocalSegmentsDetail.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains a vector of segment local poses for a single subject. -class VLocalSegments : public VItem -{ -public: - - /// Subject identifier - ViconCGStreamType::UInt32 m_SubjectID; - - /// Local segments - std::vector< ViconCGStreamDetail::VLocalSegments_Segment > m_Segments; - - // Equality operator - bool operator == ( const VLocalSegments & i_rOther ) const - { - return m_Segments == i_rOther.m_Segments; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::LocalSegments; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return m_SubjectID; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - if( !i_rBuffer.Read( m_SubjectID ) ) - { - return false; - } - - if( !i_rBuffer.Read( m_Segments ) ) - { - return false; - } - - return true; - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_SubjectID ); - i_rBuffer.Write( m_Segments ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/LocalSegmentsDetail.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/LocalSegmentsDetail.h deleted file mode 100644 index 244f55454..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/LocalSegmentsDetail.h +++ /dev/null @@ -1,80 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the detail declaration of the ViconCGStream::VLocalSegments class. - -#include "Enum.h" -#include -#include "IsEqual.h" - -namespace ViconCGStreamDetail -{ -//------------------------------------------------------------------------------------------------- - -/** - * Contains a segment translation from the parent origin, and rotation matrix (row major format). - */ -class VLocalSegments_Segment -{ -public: - /// Segment identifier - ViconCGStreamType::UInt32 m_SegmentID; - - /// Segment translation from parent origin - ViconCGStreamType::Double m_Translation[ 3 ]; - - /// Segment rotation matrix (row major format) - ViconCGStreamType::Double m_Rotation[ 9 ]; - - // Equality operator - bool operator == ( const VLocalSegments_Segment & i_rOther ) const - { - return m_SegmentID == i_rOther.m_SegmentID && - IsEqual( m_Translation, i_rOther.m_Translation ) && - IsEqual( m_Rotation, i_rOther.m_Rotation ); - } - - /// Read function. - bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_SegmentID ) && - i_rBuffer.Read( m_Translation ) && - i_rBuffer.Read( m_Rotation ); - } - - /// Write function. - void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_SegmentID ); - i_rBuffer.Write( m_Translation ); - i_rBuffer.Write( m_Rotation ); - } -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/MomentFrame.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/MomentFrame.h deleted file mode 100644 index 7821d5ec2..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/MomentFrame.h +++ /dev/null @@ -1,97 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VMomentFrame class. - -#include "Item.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains a frame number, device and channel identifiers, and a frame of moment samples in component order (measured in N/mm). -class VMomentFrame : public VItem -{ -public: - - /// Device frame number - ViconCGStreamType::UInt32 m_FrameID; - - /// Device identigier - ViconCGStreamType::UInt32 m_DeviceID; - - /// Channel identifier - ViconCGStreamType::UInt32 m_ChannelID; - - /// Moment samples (in component order) - std::vector< ViconCGStreamType::Float > m_Samples; - - /// Equality operator - bool operator == ( const VMomentFrame & i_rOther ) const - { - return m_FrameID == i_rOther.m_FrameID && - m_DeviceID == i_rOther.m_DeviceID && - m_ChannelID == i_rOther.m_ChannelID && - m_Samples == i_rOther.m_Samples; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::MomentFrame; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return m_DeviceID; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_FrameID ) && - i_rBuffer.Read( m_DeviceID ) && - i_rBuffer.Read( m_ChannelID ) && - i_rBuffer.Read( m_Samples ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_FrameID ); - i_rBuffer.Write( m_DeviceID ); - i_rBuffer.Write( m_ChannelID ); - i_rBuffer.Write( m_Samples ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/ObjectEnums.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/ObjectEnums.h deleted file mode 100644 index 3789d473f..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/ObjectEnums.h +++ /dev/null @@ -1,79 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VObjectEnums class. - -#include "Item.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Used by the server to declare the available object identifiers, and by the client to enable/disable objects. -class VObjectEnums : public VItem -{ -public: - - /// Object enums - std::set< ViconCGStreamType::Enum > m_Enums; - - // Equality operator - bool operator == ( const VObjectEnums & i_rOther ) const - { - return m_Enums == i_rOther.m_Enums; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::ObjectEnums; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return FILTER_NA; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_Enums ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_Enums ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/ObjectQuality.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/ObjectQuality.h deleted file mode 100644 index cee60667a..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/ObjectQuality.h +++ /dev/null @@ -1,85 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VObjectQuality class. - -#include "Item.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains a quality estimate for a single object. -class VObjectQuality : public VItem -{ -public: - - /// Subject identifier - ViconCGStreamType::UInt32 m_SubjectID; - - /// Quality estimate from 0.0 (bad) to 1.0 (excellent). - ViconCGStreamType::Double m_ObjectQuality; - - /// Equality operator - bool operator == ( const VObjectQuality& i_rOther ) const - { - return ( m_SubjectID == i_rOther.m_SubjectID ) && - ( m_ObjectQuality == i_rOther.m_ObjectQuality ); - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::ObjectQuality; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return m_SubjectID; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_SubjectID ) && - i_rBuffer.Read( m_ObjectQuality ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_SubjectID ); - i_rBuffer.Write( m_ObjectQuality ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/ReconRayAssignmentsDetail.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/ReconRayAssignmentsDetail.h deleted file mode 100644 index 0307b51f2..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/ReconRayAssignmentsDetail.h +++ /dev/null @@ -1,119 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStreamDetail::VRayAssignments classes. - -#include "Enum.h" -#include -#include - -namespace ViconCGStreamDetail -{ -//------------------------------------------------------------------------------------------------- - -/// (Used int VReconRayAssignments) Contains all the ray assignments for a single recon -class VReconRay -{ -public: - - /// Camera frame number - ViconCGStreamType::UInt32 m_FrameID; - - /// Camera ID of the Ray - ViconCGStreamType::UInt32 m_CameraID; - - /// Index into centroid array for the camera frame - ViconCGStreamType::UInt32 m_CentroidIndex; - - /// Equality operator - bool operator == ( const VReconRay & i_rOther ) const - { - return m_FrameID == i_rOther.m_FrameID && - m_CameraID == i_rOther.m_CameraID && - m_CentroidIndex == i_rOther.m_CentroidIndex; - } - - /// Read function. - bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_FrameID ) && - i_rBuffer.Read( m_CameraID ) && - i_rBuffer.Read( m_CentroidIndex ); - } - - /// Write function. - void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_FrameID ); - i_rBuffer.Write( m_CameraID ); - i_rBuffer.Write( m_CentroidIndex ); - } -}; - -/// (Used int VUnlabeledReconRayAssigments and VLabeledReconRayAssigments) Contains all the ray assignments for a single recon -class VReconRayAssignments -{ -public: - - /// Subject identifier - ViconCGStreamType::UInt32 m_SubjectID; - - /// Marker identifier (for subject) - ViconCGStreamType::UInt32 m_MarkerID; - - /// Vector of ray identifiers - std::vector< VReconRay > m_ReconRays; - - /// Equality operator - bool operator == ( const VReconRayAssignments & i_rOther ) const - { - return m_SubjectID == i_rOther.m_SubjectID && - m_MarkerID == i_rOther.m_MarkerID && - m_ReconRays == i_rOther.m_ReconRays; - } - - /// Read function. - bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_SubjectID ) && - i_rBuffer.Read( m_MarkerID ) && - i_rBuffer.Read( m_ReconRays ); - } - - /// Write function. - void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_SubjectID ); - i_rBuffer.Write( m_MarkerID ); - i_rBuffer.Write( m_ReconRays ); - } -}; - -//------------------------------------------------------------------------------------------------- -} - - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/RequestFrame.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/RequestFrame.h deleted file mode 100644 index 30fbd82d6..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/RequestFrame.h +++ /dev/null @@ -1,76 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -#include "Item.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Used by the client to request a new frame of data and to control streaming mode -class VRequestFrame : public VItem -{ -public: - - /// Streaming mode - bool m_bStreaming; - - /// Equality operator - bool operator == ( const VRequestFrame & i_rOther ) const - { - return m_bStreaming == i_rOther.m_bStreaming; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::RequestFrame; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return FILTER_NA; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_bStreaming ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_bStreaming ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/ScopedReader.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/ScopedReader.h deleted file mode 100644 index e054afcc1..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/ScopedReader.h +++ /dev/null @@ -1,108 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -#include -#include "Enum.h" -#include -#include - -namespace ViconCGStream -{ - class VItem; -} - -namespace ViconCGStreamIO -{ -//------------------------------------------------------------------------------------------------- - -/** \class VScopedReader -This class provides functionality for reading nested blocks of data from a buffer. -Each block has a header which contains a ViconCGStreamEnum identifier, and a block length. -**/ - -class VScopedReader -{ -public: - - /// Constructor taking the underlying buffer to read from. - VScopedReader( VBuffer & i_rBuffer ) - : m_rBuffer( i_rBuffer ) - , m_Enum( 0 ) - , m_Start( 0 ) - , m_End( 0 ) - { - ViconCGStreamType::UInt32 Length; - - if( !m_rBuffer.Read( m_Enum ) || !m_rBuffer.Read( Length ) ) - { - return; - } - - m_Start = m_rBuffer.Offset(); - m_End = m_Start + Length; - } - - /// Destructor. - ~VScopedReader() - { - if( m_Start < m_End ) - { - m_rBuffer.SetOffset( m_End ); - } - } - - /// Determine if the current buffer offset is valid. - bool Ok() - { - ViconCGStreamType::UInt32 Offset = m_rBuffer.Offset(); - return Offset >= m_Start && Offset < m_End; - } - - /// Get the Enum read from the block header. - /// This function is used to determine the block type. - /// If the block is valid, then the value returned will be defined in the ViconCGStreamEnum namespace. - ViconCGStreamType::Enum Enum() const - { - return m_Enum; - } - - /// Read a single item from the buffer. - bool Read( ViconCGStream::VItem & o_rItem ) const - { - return m_rBuffer.Read( o_rItem ); - } - -private: - VBuffer& m_rBuffer; - ViconCGStreamType::Enum m_Enum; - ViconCGStreamType::UInt32 m_Start; - ViconCGStreamType::UInt32 m_End; -}; - -//------------------------------------------------------------------------------------------------- -}; - - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/ScopedWriter.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/ScopedWriter.h deleted file mode 100644 index 290cc1c0c..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/ScopedWriter.h +++ /dev/null @@ -1,79 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -#include "Item.h" -#include -#include - -namespace ViconCGStreamIO -{ -//------------------------------------------------------------------------------------------------- - -/** \class VScopedWriter -This class provides functionality for writing nested blocks of data to a buffer. -Each block has a header which contains a ViconCGStreamEnum identifier, and a block length. -**/ - -class VScopedWriter -{ -public: - - /// Constructor taking the underlying buffer to write to and the enum for this block. - VScopedWriter( VBuffer & i_rBuffer, ViconCGStreamType::Enum i_Enum = ViconCGStreamEnum::Objects ) - : m_rBuffer( i_rBuffer ) - { - m_rBuffer.Write( i_Enum ); - m_Start = m_rBuffer.Offset(); - m_rBuffer.SetOffset( m_Start + sizeof( ViconCGStreamType::UInt32 ) ); - } - - /// Destructor that fills out length of block in header. - ~VScopedWriter() - { - ViconCGStreamType::UInt32 Offset = m_rBuffer.Offset(); - ViconCGStreamType::UInt32 Length = ( Offset - m_Start ) - sizeof( ViconCGStreamType::UInt32 ); - - m_rBuffer.SetOffset( m_Start ); - m_rBuffer.Write( Length ); - m_rBuffer.SetOffset( Offset ); - } - - /// Write an item and its enum type to the buffer. - void Write( const ViconCGStream::VItem & i_rItem ) - { - VScopedWriter ScopedWriter( m_rBuffer, i_rItem.TypeID() ); - m_rBuffer.Write( i_rItem ); - } - -private: - VBuffer & m_rBuffer; - ViconCGStreamType::UInt32 m_Start; -}; - -//------------------------------------------------------------------------------------------------- -}; - - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/StartMulticastSender.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/StartMulticastSender.h deleted file mode 100644 index 5a0e4119a..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/StartMulticastSender.h +++ /dev/null @@ -1,88 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -#include "Item.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Start client multicast session -class VStartMulticastSender : public VItem -{ -public: - - /// Destination IP address, should be multicast or broadcast - ViconCGStreamType::UInt32 m_MulticastIpAddress; - - // Source IP address - ViconCGStreamType::UInt32 m_SourceIpAddress; - - // Port number - ViconCGStreamType::UInt16 m_Port; - - /// Equality operator - bool operator == ( const VStartMulticastSender & i_rOther ) const - { - return m_MulticastIpAddress == i_rOther.m_MulticastIpAddress - && m_SourceIpAddress == i_rOther.m_SourceIpAddress - && m_Port == i_rOther.m_Port; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::StartMulticastSender; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return FILTER_NA; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_MulticastIpAddress ) - && i_rBuffer.Read( m_SourceIpAddress ) - && i_rBuffer.Read( m_Port ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_MulticastIpAddress ); - i_rBuffer.Write( m_SourceIpAddress ); - i_rBuffer.Write( m_Port );; - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/StopMulticastSender.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/StopMulticastSender.h deleted file mode 100644 index 479c29546..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/StopMulticastSender.h +++ /dev/null @@ -1,74 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VKeepAlive class. - -#include "Item.h" - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Stop client multicast session -class VStopMulticastSender : public VItem -{ -public: - - /// Equality operator. - bool operator == ( const VStopMulticastSender & /*i_rVStopMulticastSender*/ ) const - { - return true; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::StopMulticastSender; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return FILTER_NA; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & /*i_rBuffer*/ ) - { - return true; - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & /*i_rBuffer*/ ) const - { - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/StreamInfo.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/StreamInfo.h deleted file mode 100644 index 00fc2173d..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/StreamInfo.h +++ /dev/null @@ -1,78 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VStreamInfo class. - -#include "Item.h" - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains the master frame period. -class VStreamInfo : public VItem -{ -public: - - /// Stream frame period. Defined as the numbers of ticks per frame on a 135Mhz clock. - ViconCGStreamType::Int64 m_FramePeriod; - - /// Equality operator - bool operator == ( const VStreamInfo& i_rOther ) const - { - return m_FramePeriod == i_rOther.m_FramePeriod; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::StreamInfo; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return FILTER_NA; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_FramePeriod ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_FramePeriod ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/SubjectHealth.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/SubjectHealth.h deleted file mode 100644 index cf4a47fc5..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/SubjectHealth.h +++ /dev/null @@ -1,91 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VSubjectHealth class. - -#include "Item.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains a health estimate for a single subject. -class VSubjectHealth : public VItem -{ -public: - - /// Subject identifier - ViconCGStreamType::UInt32 m_SubjectID; - - /// Residual error computed from reconstructions, in mm. - ViconCGStreamType::Double m_MarkerError; - - /// Health estimate from 0.0 (bad) to 1.0 (excellent). - ViconCGStreamType::Double m_SubjectHealth; - - /// Equality operator - bool operator == ( const VSubjectHealth& i_rOther ) const - { - return ( m_SubjectID == i_rOther.m_SubjectID ) && - ( m_MarkerError == i_rOther.m_MarkerError ) && - ( m_SubjectHealth == i_rOther.m_SubjectHealth ); - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::SubjectHealth; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return m_SubjectID; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_SubjectID ) && - i_rBuffer.Read( m_MarkerError ) && - i_rBuffer.Read( m_SubjectHealth ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_SubjectID ); - i_rBuffer.Write( m_MarkerError ); - i_rBuffer.Write( m_SubjectHealth ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/SubjectInfo.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/SubjectInfo.h deleted file mode 100644 index e615c77ac..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/SubjectInfo.h +++ /dev/null @@ -1,104 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VSubjectInfo class. - -#include "Item.h" -#include "SubjectInfoDetail.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains the static description of a single subject. -class VSubjectInfo : public VItem -{ -public: - - /// Subject identifier - ViconCGStreamType::UInt32 m_SubjectID; - - /// Subject name - std::string m_Name; - - /// Segments - std::vector< ViconCGStreamDetail::VSubjectInfo_Segment > m_Segments; - - /// Markers - std::vector< ViconCGStreamDetail::VSubjectInfo_Marker > m_Markers; - - /// Marker to segment attachments - std::vector< ViconCGStreamDetail::VSubjectInfo_Attachment > m_Attachments; - - /// Equality operator - bool operator == ( const VSubjectInfo& i_rOther ) const - { - return ( m_SubjectID == i_rOther.m_SubjectID ) && - ( m_Name == i_rOther.m_Name ) && - ( m_Segments == i_rOther.m_Segments ) && - ( m_Markers == i_rOther.m_Markers ) && - ( m_Attachments == i_rOther.m_Attachments ); - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::SubjectInfo; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return m_SubjectID; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_SubjectID ) && - i_rBuffer.Read( m_Name ) && - i_rBuffer.Read( m_Segments ) && - i_rBuffer.Read( m_Markers ) && - i_rBuffer.Read( m_Attachments ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_SubjectID ); - i_rBuffer.Write( m_Name ); - i_rBuffer.Write( m_Segments ); - i_rBuffer.Write( m_Markers ); - i_rBuffer.Write( m_Attachments ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/SubjectInfoDetail.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/SubjectInfoDetail.h deleted file mode 100644 index f0525c939..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/SubjectInfoDetail.h +++ /dev/null @@ -1,170 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the details declaration of the ViconCGStream::VSubjectInfo class. - -#include "Enum.h" -#include -#include "IsEqual.h" - -namespace ViconCGStreamDetail -{ -//------------------------------------------------------------------------------------------------- - -/// Contains a segment name, segment and parent segment identifiers, and segment bounding box. -class VSubjectInfo_Segment -{ -public: - /// Parent segment identifier (or zero) - ViconCGStreamType::UInt32 m_ParentID; - - /// Segment identifier - ViconCGStreamType::UInt32 m_SegmentID; - - /// Segment axis aligned bounds (min and max) - ViconCGStreamType::Float m_Bounds[ 6 ]; - - /// Segment name - std::string m_Name; - - /// Equality function - bool IsEqual( const VSubjectInfo_Segment & i_rOther ) const - { - return m_ParentID == i_rOther.m_ParentID && - m_SegmentID == i_rOther.m_SegmentID && - m_Name == i_rOther.m_Name && - ViconCGStreamDetail::IsEqual( m_Bounds, i_rOther.m_Bounds ); - } - - /// Equality operator - bool operator == ( const VSubjectInfo_Segment & i_rOther ) const - { - return IsEqual( i_rOther ); - } - - /// Read function. - bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_ParentID ) && - i_rBuffer.Read( m_SegmentID ) && - i_rBuffer.Read( m_Bounds ) && - i_rBuffer.Read( m_Name ); - } - - /// Write function. - void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_ParentID ); - i_rBuffer.Write( m_SegmentID ); - i_rBuffer.Write( m_Bounds ); - i_rBuffer.Write( m_Name ); - } -}; - -/// Contains a marker name and identifier. -class VSubjectInfo_Marker -{ -public: - /// Marker identifier - ViconCGStreamType::UInt32 m_MarkerID; - - /// Marker name - std::string m_Name; - - /// Equality function - bool IsEqual( const VSubjectInfo_Marker & i_rOther ) const - { - return m_MarkerID == i_rOther.m_MarkerID && - m_Name == i_rOther.m_Name; - } - - /// Equality operator - bool operator == ( const VSubjectInfo_Marker & i_rOther ) const - { - return IsEqual( i_rOther ); - } - - /// Read function. - bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_MarkerID ) && - i_rBuffer.Read( m_Name ); - } - - /// Write function. - void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_MarkerID ); - i_rBuffer.Write( m_Name ); - } - -}; - -/// (Member of VSubjectInfo) Contains a marker identifier, and parent segment identifier. -class VSubjectInfo_Attachment -{ -public: - - /// Maker identifier - ViconCGStreamType::UInt32 m_MarkerID; - - /// Parent segment identifier - ViconCGStreamType::UInt32 m_SegmentID; - - /// Equality function - bool IsEqual( const VSubjectInfo_Attachment & i_rOther ) const - { - return m_MarkerID == i_rOther.m_MarkerID && - m_SegmentID == i_rOther.m_SegmentID; - } - - /// Equality operator - bool operator == ( const VSubjectInfo_Attachment & i_rOther ) const - { - return IsEqual( i_rOther ); - } - - /// Read function. - bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_MarkerID ) && - i_rBuffer.Read( m_SegmentID ); - } - - /// Write function. - void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_MarkerID ); - i_rBuffer.Write( m_SegmentID ); - } - -}; - -//------------------------------------------------------------------------------------------------- - -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/SubjectTopology.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/SubjectTopology.h deleted file mode 100644 index 977fa2ca3..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/SubjectTopology.h +++ /dev/null @@ -1,92 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VSubjectCalibrationInfo class. - -#include -#include "SubjectTopologyDetail.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains the static description of a single subject. -class VSubjectTopology : public VItem -{ -public: - - /// Subject identifier - ViconCGStreamType::UInt32 m_SubjectID; - - /// Segments - std::vector< ViconCGStreamDetail::VSubjectTopology_Segment > m_Segments; - - /// Equality function - bool IsEqual( const VSubjectTopology & i_rOther ) const - { - return m_SubjectID == i_rOther.m_SubjectID && - m_Segments == i_rOther.m_Segments; - } - - /// Equality operator - bool operator == ( const VSubjectTopology & i_rOther ) const - { - return IsEqual( i_rOther ); - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::SubjectTopology; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return m_SubjectID; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_SubjectID ) && - i_rBuffer.Read( m_Segments ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_SubjectID ); - i_rBuffer.Write( m_Segments ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/SubjectTopologyDetail.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/SubjectTopologyDetail.h deleted file mode 100644 index 95470df67..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/SubjectTopologyDetail.h +++ /dev/null @@ -1,79 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -#include -#include -#include - -namespace ViconCGStreamDetail -{ -//------------------------------------------------------------------------------------------------- - -/// (Member of VSubjectTopology) Contains neutral pose information -class VSubjectTopology_Segment -{ -public: - /// Segment identifier - ViconCGStreamType::UInt32 m_SegmentID; - - /// Translation in parent segment at neutral pose - ViconCGStreamType::Double m_Translation[ 3 ]; - - /// Rotation in parent segment at neutral pose - ViconCGStreamType::Double m_Rotation[ 9 ]; - - bool IsEqual( const VSubjectTopology_Segment & i_rOther ) const - { - return m_SegmentID == i_rOther.m_SegmentID && - ViconCGStreamDetail::IsEqual( m_Translation, i_rOther.m_Translation ) && - ViconCGStreamDetail::IsEqual( m_Rotation, i_rOther.m_Rotation ); - } - - /// Equality operator - bool operator == ( const VSubjectTopology_Segment & i_rOther ) const - { - return IsEqual( i_rOther ); - } - - /// Read function. - bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_SegmentID ) && - i_rBuffer.Read( m_Translation ) && - i_rBuffer.Read( m_Rotation ); - } - - /// Write function. - void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_SegmentID ); - i_rBuffer.Write( m_Translation ); - i_rBuffer.Write( m_Rotation ); - } -}; - -//------------------------------------------------------------------------------------------------- -}; diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/Timecode.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/Timecode.h deleted file mode 100644 index 298146e2f..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/Timecode.h +++ /dev/null @@ -1,133 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VTimecode class. - -#include "Item.h" - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -class VTimecode : public VItem -{ -public: - - /// Time code standard - enum ETimecodeStandard - { - ETimecodePAL = 0, ///< PAL - ETimecodeNTSC, ///< NTSC - ETimecodeNTSCDrop, ///< NTSCDrop - ETimecodeFilm, ///< Film at 24fps - ETimecodeNTSCFilm, ///< NTSC Film (23.98fps) - ETimecodeATSC, ///< ATSC (30fps) - ETimecodeStandardCount, ///< Standard - ETimecodeNone = -1 ///< Invalid - }; - - /// Timecode - ViconCGStreamType::UInt8 m_Hours; - ViconCGStreamType::UInt8 m_Minutes; - ViconCGStreamType::UInt8 m_Seconds; - ViconCGStreamType::UInt8 m_Frames; - ViconCGStreamType::UInt16 m_Subframes; - ViconCGStreamType::UInt16 m_SubframesPerFrame; - ViconCGStreamType::UInt32 m_UserBits; - ViconCGStreamType::UInt8 m_FieldFlag; - ViconCGStreamType::UInt32 m_Standard; - - VTimecode() - : m_Hours( 0 ) - , m_Minutes( 0 ) - , m_Seconds( 0 ) - , m_Frames( 0 ) - , m_Subframes( 0 ) - , m_SubframesPerFrame( 0 ) - , m_UserBits( 0 ) - , m_FieldFlag( 0 ) - , m_Standard( ETimecodeNone ) - { - } - - /// Equality operator - bool operator == ( const VTimecode & i_rOther ) const - { - return m_Hours == i_rOther.m_Hours && - m_Minutes == i_rOther.m_Minutes && - m_Seconds == i_rOther.m_Seconds && - m_Frames == i_rOther.m_Frames && - m_Subframes == i_rOther.m_Subframes && - m_SubframesPerFrame == i_rOther.m_SubframesPerFrame && - m_UserBits == i_rOther.m_UserBits && - m_FieldFlag == i_rOther.m_FieldFlag && - m_Standard == i_rOther.m_Standard; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::Timecode; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return FILTER_NA; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_Hours ) && - i_rBuffer.Read( m_Minutes ) && - i_rBuffer.Read( m_Seconds ) && - i_rBuffer.Read( m_Frames ) && - i_rBuffer.Read( m_Subframes ) && - i_rBuffer.Read( m_SubframesPerFrame ) && - i_rBuffer.Read( m_Standard ) && - i_rBuffer.Read( m_UserBits ) && - i_rBuffer.Read( m_FieldFlag ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_Hours ); - i_rBuffer.Write( m_Minutes ); - i_rBuffer.Write( m_Seconds ); - i_rBuffer.Write( m_Frames ); - i_rBuffer.Write( m_Subframes ); - i_rBuffer.Write( m_SubframesPerFrame ); - i_rBuffer.Write( m_Standard ); - i_rBuffer.Write( m_UserBits ); - i_rBuffer.Write( m_FieldFlag ); - } -}; - -//------------------------------------------------------------------------------------------------- -} diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/UnlabeledReconRayAssignments.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/UnlabeledReconRayAssignments.h deleted file mode 100644 index 65cd8e096..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/UnlabeledReconRayAssignments.h +++ /dev/null @@ -1,80 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VUnlabeledReconRayAssignments class. - -#include "Item.h" -#include "ReconRayAssignmentsDetail.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Unlabeled ray assignments -class VUnlabeledReconRayAssignments : public VItem -{ -public: - - /// Recon ray assignments. - std::vector< ViconCGStreamDetail::VReconRayAssignments > m_ReconRayAssignments; - - /// Equality operator. - bool operator == ( const VUnlabeledReconRayAssignments & i_rOther ) - { - return m_ReconRayAssignments == i_rOther.m_ReconRayAssignments; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const { - return ViconCGStreamEnum::UnlabeledReconRayAssignments; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return FILTER_NA; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_ReconRayAssignments ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_ReconRayAssignments ); - } - -}; - -//------------------------------------------------------------------------------------------------- -} - - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/UnlabeledRecons.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/UnlabeledRecons.h deleted file mode 100644 index 69293b695..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/UnlabeledRecons.h +++ /dev/null @@ -1,81 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VUnlabeledRecons class. - -#include "Item.h" -#include "UnlabeledReconsDetail.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains a frame of unlabeled recons, consisting of radii, positions and covariances. -class VUnlabeledRecons : public VItem -{ -public: - - /// Unlabeled recons - std::vector< ViconCGStreamDetail::VUnlabeledRecons_UnlabeledRecon > m_UnlabeledRecons; - - /// Equality operator - bool operator == ( const VUnlabeledRecons & i_rOther ) const - { - return m_UnlabeledRecons == i_rOther.m_UnlabeledRecons; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::UnlabeledRecons; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return FILTER_NA; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_UnlabeledRecons ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_UnlabeledRecons ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/UnlabeledReconsDetail.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/UnlabeledReconsDetail.h deleted file mode 100644 index e5f0ad097..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/UnlabeledReconsDetail.h +++ /dev/null @@ -1,85 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the details declaration of the ViconCGStream::VUnlabeledRecons class. - -#include "Enum.h" -#include -#include "IsEqual.h" - -namespace ViconCGStreamDetail -{ -//------------------------------------------------------------------------------------------------- - -/// Contains a single unlabeled recon. -class VUnlabeledRecons_UnlabeledRecon -{ -public: - /// Recon radius - ViconCGStreamType::Double m_Radius; - - /// Recon position - ViconCGStreamType::Double m_Position[ 3 ]; - - /// Recon covariance matrix (row major format) - ViconCGStreamType::Double m_Covariance[ 9 ]; - - /// Trajectory id - ViconCGStreamType::UInt32 m_TrajectoryId; - - /// Equality operator - bool operator == ( const VUnlabeledRecons_UnlabeledRecon & i_rOther ) const - { - return m_Radius == i_rOther.m_Radius && - IsEqual( m_Position, i_rOther.m_Position ) && - IsEqual( m_Covariance, i_rOther.m_Covariance ) && - m_TrajectoryId == i_rOther.m_TrajectoryId; - } - - /// Read function. - bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_Radius ) && - i_rBuffer.Read( m_Position ) && - i_rBuffer.Read( m_Covariance ) && - i_rBuffer.Read( m_TrajectoryId ); - } - - /// Write function. - void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_Radius ); - i_rBuffer.Write( m_Position ); - i_rBuffer.Write( m_Covariance ); - i_rBuffer.Write( m_TrajectoryId ); - } -}; - -//------------------------------------------------------------------------------------------------- -}; - - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/VideoFrame.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/VideoFrame.h deleted file mode 100644 index 3bf4b74e0..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/VideoFrame.h +++ /dev/null @@ -1,119 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VVideoFrame class. - -#include "Item.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains a frame of video, along with the camera identifier and frame number. -class VVideoFrame : public VItem -{ -public: - - /// Camera frame number - ViconCGStreamType::UInt32 m_FrameID; - - /// Camera identifier - ViconCGStreamType::UInt32 m_CameraID; - - /// Video frame window offset - ViconCGStreamType::Int16 m_Position[ 2 ]; - - /// Video frame window width - ViconCGStreamType::UInt16 m_Width; - - /// Video frame window height - ViconCGStreamType::UInt16 m_Height; - - // Video frame format - enum EFormat { ENoVideo = 0, EMono8, EBayerRG8, EBayerGB8, EBayerGR8, EBayerBG8, ERGB888, EBGR888 }; - ViconCGStreamType::UInt32 m_Format; - - /// Video data. - /// The first pixel in this buffer should be displayed in the top-left position when rendered. - std::vector< ViconCGStreamType::UInt8 > m_VideoData; - - /// Equality operator - bool operator == ( const VVideoFrame & i_rOther ) const - { - return m_FrameID == i_rOther.m_FrameID && - m_CameraID == i_rOther.m_CameraID && - m_Position[ 0 ] == i_rOther.m_Position[ 0 ] && - m_Position[ 1 ] == i_rOther.m_Position[ 1 ] && - m_Width == i_rOther.m_Width && - m_Height == i_rOther.m_Height && - m_Format == i_rOther.m_Format && - m_VideoData == i_rOther.m_VideoData; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::VideoFrame; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return m_CameraID; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_FrameID ) && - i_rBuffer.Read( m_CameraID ) && - i_rBuffer.Read( m_Position ) && - i_rBuffer.Read( m_Width ) && - i_rBuffer.Read( m_Height ) && - i_rBuffer.Read( m_Format ) && - i_rBuffer.Read( m_VideoData ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_FrameID ); - i_rBuffer.Write( m_CameraID ); - i_rBuffer.Write( m_Position[ 0 ] ); - i_rBuffer.Write( m_Position[ 1 ] ); - i_rBuffer.Write( m_Width ); - i_rBuffer.Write( m_Height ); - i_rBuffer.Write( m_Format ); - i_rBuffer.Write( m_VideoData ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/VoltageFrame.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/VoltageFrame.h deleted file mode 100644 index 33042ae12..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/VoltageFrame.h +++ /dev/null @@ -1,97 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStream::VVoltageFrame class. - -#include "Item.h" -#include - -namespace ViconCGStream -{ -//------------------------------------------------------------------------------------------------- - -/// Contains a frame number, device and channel identifiers, and a frame of voltages in component order (measured in V). -class VVoltageFrame : public VItem -{ -public: - - /// Device frame number - ViconCGStreamType::UInt32 m_FrameID; - - /// Device iedntifier - ViconCGStreamType::UInt32 m_DeviceID; - - /// Channel identifier - ViconCGStreamType::UInt32 m_ChannelID; - - /// Voltage samples (in component order) - std::vector< ViconCGStreamType::Float > m_Samples; - - /// Equality operator - bool operator == ( const VVoltageFrame & i_rOther ) const - { - return m_FrameID == i_rOther.m_FrameID && - m_DeviceID == i_rOther.m_DeviceID && - m_ChannelID == i_rOther.m_ChannelID && - m_Samples == i_rOther.m_Samples; - } - - /// Object type enum. - virtual ViconCGStreamType::Enum TypeID() const - { - return ViconCGStreamEnum::VoltageFrame; - } - - /// Filter ID - virtual ViconCGStreamType::UInt32 FilterID() const - { - return m_DeviceID; - } - - /// Read function. - virtual bool Read( const ViconCGStreamIO::VBuffer & i_rBuffer ) - { - return i_rBuffer.Read( m_FrameID ) && - i_rBuffer.Read( m_DeviceID ) && - i_rBuffer.Read( m_ChannelID ) && - i_rBuffer.Read( m_Samples ); - } - - /// Write function. - virtual void Write( ViconCGStreamIO::VBuffer & i_rBuffer ) const - { - i_rBuffer.Write( m_FrameID ); - i_rBuffer.Write( m_DeviceID ); - i_rBuffer.Write( m_ChannelID ); - i_rBuffer.Write( m_Samples ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/makefile b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/makefile deleted file mode 100644 index 6353d6a54..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStream/makefile +++ /dev/null @@ -1,108 +0,0 @@ -# Makefile for ViconCGStream - -ifndef VERBOSE -.SILENT : -endif -.SUFFIXES : - -ifdef CONFIG -ifneq ($(CONFIG),Debug) -ifneq ($(CONFIG),InternalRelease) -ifneq ($(CONFIG),Release) -Error: unknown configuration. -endif -endif -endif -else -CONFIG=Debug -endif - -INCLUDEPATHS=. .. ../../../../thirdparty/Boost/boost-1.58.0-dynamic-linux-x64/installed/include ../.. -INCLUDEPATHS_Debug=Debug -INCLUDEPATHS_InternalRelease=InternalRelease -INCLUDEPATHS_Release=Release -LIBRARYPATHS=../../../../thirdparty/Boost/boost-1.58.0-dynamic-linux-x64/installed/lib -LIBRARYPATHS_Debug=../../../../lib/Debug ../../../../bin/Debug -LIBRARYPATHS_InternalRelease=../../../../lib/InternalRelease ../../../../bin/InternalRelease -LIBRARYPATHS_Release=../../../../lib/Release ../../../../bin/Release -DEPENDENCIES=StreamCommon -LIBRARIES= -LIBRARIES_Debug=boost_wserialization-mt-d boost_wave-mt-d boost_unit_test_framework-mt-d boost_timer-mt-d boost_thread-mt-d boost_system-mt-d boost_signals-mt-d boost_serialization-mt-d boost_regex-mt-d boost_random-mt-d boost_python-mt-d boost_program_options-mt-d boost_prg_exec_monitor-mt-d boost_math_tr1l-mt-d boost_math_tr1f-mt-d boost_math_tr1-mt-d boost_math_c99l-mt-d boost_math_c99f-mt-d boost_math_c99-mt-d boost_log_setup-mt-d boost_log-mt-d boost_locale-mt-d boost_iostreams-mt-d boost_graph-mt-d boost_filesystem-mt-d boost_date_time-mt-d boost_coroutine-mt-d boost_context-mt-d boost_container-mt-d boost_chrono-mt-d boost_atomic-mt-d -LIBRARIES_InternalRelease=boost_wserialization-mt boost_wave-mt boost_unit_test_framework-mt boost_timer-mt boost_thread-mt boost_system-mt boost_signals-mt boost_serialization-mt boost_regex-mt boost_random-mt boost_python-mt boost_program_options-mt boost_prg_exec_monitor-mt boost_math_tr1l-mt boost_math_tr1f-mt boost_math_tr1-mt boost_math_c99l-mt boost_math_c99f-mt boost_math_c99-mt boost_log_setup-mt boost_log-mt boost_locale-mt boost_iostreams-mt boost_graph-mt boost_filesystem-mt boost_date_time-mt boost_coroutine-mt boost_context-mt boost_container-mt boost_chrono-mt boost_atomic-mt -LIBRARIES_Release=boost_wserialization-mt boost_wave-mt boost_unit_test_framework-mt boost_timer-mt boost_thread-mt boost_system-mt boost_signals-mt boost_serialization-mt boost_regex-mt boost_random-mt boost_python-mt boost_program_options-mt boost_prg_exec_monitor-mt boost_math_tr1l-mt boost_math_tr1f-mt boost_math_tr1-mt boost_math_c99l-mt boost_math_c99f-mt boost_math_c99-mt boost_log_setup-mt boost_log-mt boost_locale-mt boost_iostreams-mt boost_graph-mt boost_filesystem-mt boost_date_time-mt boost_coroutine-mt boost_context-mt boost_container-mt boost_chrono-mt boost_atomic-mt -DEFINES=TCM_LINUX TCM_UNIX PROJECT_SOURCE_PATH=\"/home/swtest/workspace/DatastreamSDK_Linux_64_Branch_Release/Binary/bin/Release/deployment/Vicon/CrossMarket/DataStream/ViconCGStream\" -DEFINES_Debug=_DEBUG -DEFINES_InternalRelease=VICON_INTERNAL_RELEASE NDEBUG -DEFINES_Release=NDEBUG TCM_OFF_SITE -ENV_CPU=x64 - -SOURCEDIRECTORY=../../../.. -PROJECTPATH=. -BINARYDIRECTORY=../../../.. -INTERMEDIATEDIRECTORY=. -LIBRARYDIRECTORY=../../../../lib -OUTPUTDIRECTORY=../../../../bin - -include $(BINARYDIRECTORY)/gcc.mk - -HIDE_BOOST_SCRIPT=hide_boost_version_script -ifneq ($(HIDE_BOOST),) - HIDE_BOOST_LD_PARAM= -Wl,--version-script=$(HIDE_BOOST_SCRIPT) - HIDE_BOOST_LD_PREREQ=$(HIDE_BOOST_SCRIPT) -endif -all: all_$(CONFIG) - -all_Debug: $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStream.a -all_InternalRelease: $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStream.a -all_Release: $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStream.a - -OBJECTS=$(CONFIG)/Dummy.o - -CXXFLAGS=$(CXXFLAGS_$(CONFIG)) $(L_INCLUDEPATHS) $(L_INCLUDEPATHS_$(CONFIG)) $(L_INCLUDES) $(L_INCLUDES_$(CONFIG)) $(L_DEFINES) $(L_DEFINES_$(CONFIG)) -CCFLAGS=$(CCFLAGS_$(CONFIG)) $(L_INCLUDEPATHS) $(L_INCLUDEPATHS_$(CONFIG)) $(L_INCLUDES) $(L_INCLUDES_$(CONFIG)) $(L_DEFINES) $(L_DEFINES_$(CONFIG)) -LDFLAGS=$(LDFLAGS_$(CONFIG)) $(L_LIBRARYPATHS) $(L_LIBRARYPATHS_$(CONFIG)) -CXXFLAGS+=-Wno-unused-local-typedefs -CXXFLAGS+=-Wno-delete-non-virtual-dtor -std=c++14 -#Android toolchain does not include librt but integrates some of its functionality into Android libc. -ifndef ANDROID_TARGET_ARCH -LDFLAGS+=-lrt -endif - - -$(LIBRARYDIRECTORY)/Debug/libViconCGStream.a: makefile $(OBJECTS) - @echo \[1\;35mLinking archive library $@\ - @mkdir -p $(@D) - $(AR) rcs $@ $(OBJECTS) - -$(LIBRARYDIRECTORY)/InternalRelease/libViconCGStream.a: makefile $(OBJECTS) - @echo \[1\;35mLinking archive library $@\ - @mkdir -p $(@D) - $(AR) rcs $@ $(OBJECTS) - -$(LIBRARYDIRECTORY)/Release/libViconCGStream.a: makefile $(OBJECTS) - @echo \[1\;35mLinking archive library $@\ - @mkdir -p $(@D) - $(AR) rcs $@ $(OBJECTS) - -# Source Files -$(CONFIG)/Dummy.o: makefile $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconCGStream/Dummy.cpp - @echo \[1\;34mCompiling Dummy.cpp\ - @mkdir -p $(@D) - find $(CONFIG) -name *.gch -exec cp '{}' . \; - $(CXX) -fPIC -MMD -MP -I$(CONFIG)/ $(CXXFLAGS) -o $@ -c $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconCGStream/Dummy.cpp - --include $(CONFIG)/Dummy.d - -# Header Files -# Other Files - -clean: - @echo \[1\;31mCleaning $(CONFIG) build\ - find . -path '*/$(CONFIG)/*' \( -name '*.[od]' -o -name '*.gch' \) -exec rm -f {} ';' - rm -f moc_*.cxx - -$(HIDE_BOOST_SCRIPT): makefile - echo -n >$@ - echo "{" >>$@ - echo " local: *N5boost*; *NK5boost*;" >>$@ - echo "};" >>$@ diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClient/CGStreamReaderWriter.cpp b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClient/CGStreamReaderWriter.cpp deleted file mode 100644 index 687cbcc12..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClient/CGStreamReaderWriter.cpp +++ /dev/null @@ -1,120 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#include "CGStreamReaderWriter.h" - -VCGStreamReaderWriter::VCGStreamReaderWriter( std::shared_ptr< boost::asio::ip::tcp::socket > i_pSocket ) -: m_pSocket( i_pSocket ) -{ - // linger on shutdown a bit to ensure close packet arrives - boost::system::error_code Error; - m_pSocket->set_option( boost::asio::socket_base::linger( true, 10 ), Error ); -} - -VCGStreamReaderWriter::VCGStreamReaderWriter( std::shared_ptr< boost::asio::ip::udp::socket > i_pMulticastSocket ) -: m_pMulticastSocket( i_pMulticastSocket ) -{ -} - -bool VCGStreamReaderWriter::DataReady( bool & o_rbDataReady ) const -{ - boost::asio::socket_base::bytes_readable Command( true ); - boost::system::error_code Error; - - if( m_pSocket->io_control( Command, Error ) ) - { - return false; - } - - size_t Bytes = Command.get(); - o_rbDataReady = ( Bytes != 0 ); - return true; -} - -bool VCGStreamReaderWriter::IsOpen() const -{ - return m_pSocket->is_open(); -} - -void VCGStreamReaderWriter::Close() -{ - boost::system::error_code DontCareError; - m_pSocket->shutdown( boost::asio::ip::tcp::socket::shutdown_both, DontCareError ); - m_pSocket->close(); -} - -bool VCGStreamReaderWriter::Fill() -{ - try - { - if( m_pMulticastSocket ) - { - SetLength( 64 * 1024 ); - m_pMulticastSocket->receive( boost::asio::buffer( Raw(), Length() ) ); - SetOffset( 0 ); - } - else - { - const unsigned int HeaderSize = sizeof( ViconCGStreamType::Enum ) + sizeof( ViconCGStreamType::UInt32 ); - SetLength( HeaderSize ); - SetOffset( 0 ); - boost::asio::read( *m_pSocket, boost::asio::buffer( Raw(), Length() ) ); - - SetOffset( sizeof( ViconCGStreamType::Enum ) ); - ViconCGStreamType::UInt32 BlockLength; - Read( BlockLength ); - - SetLength( Length() + BlockLength ); - boost::asio::read( *m_pSocket, boost::asio::buffer( Raw() + HeaderSize, BlockLength ) ); - SetOffset( 0 ); - } - - } - catch( boost::system::system_error & rError ) - { - std::string Error = rError.what(); - return false; - } - - return true; -} - -bool VCGStreamReaderWriter::Flush() -{ - // Will generate an error if called on when initialized with a multicast socket - try - { - SetOffset( 0 ); - boost::asio::write( *m_pSocket, boost::asio::buffer( Raw(), Length() ) ); - Clear(); - } - catch( boost::system::system_error & rError ) - { - std::string Error = rError.what(); - return false; - } - - return true; -} - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClient/CGStreamReaderWriter.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClient/CGStreamReaderWriter.h deleted file mode 100644 index 41e96cde2..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClient/CGStreamReaderWriter.h +++ /dev/null @@ -1,57 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -#include -#include -#include - -// Class providing asio based socket support for the cg stream -class VCGStreamReaderWriter : public ViconCGStreamIO::VBuffer -{ -public: - - VCGStreamReaderWriter( std::shared_ptr< boost::asio::ip::tcp::socket > i_pSocket ); - - VCGStreamReaderWriter( std::shared_ptr< boost::asio::ip::udp::socket > i_pMulticastSocket ); - - // determine if there is data ready to read. Return value indicates if an error occured - bool DataReady( bool & o_rbDataReady ) const; - - // Close the socket - void Close(); - - // Determine if socket is open - bool IsOpen() const; - - // Fill buffer from socket - bool Fill(); - - // Flush buffer to socket - bool Flush(); - - std::shared_ptr< boost::asio::ip::tcp::socket > m_pSocket; - std::shared_ptr< boost::asio::ip::udp::socket > m_pMulticastSocket; -}; diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClient/IViconCGStreamClientCallback.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClient/IViconCGStreamClientCallback.h deleted file mode 100644 index 629d1bc1a..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClient/IViconCGStreamClientCallback.h +++ /dev/null @@ -1,62 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -#include - -class VStaticObjects; -class VDynamicObjects; - -class IViconCGStreamClientCallback -{ -public: - virtual void OnConnect(); - virtual void OnStaticObjects( std::shared_ptr< const VStaticObjects > i_pStaticObjects ); - virtual void OnDynamicObjects( std::shared_ptr< const VDynamicObjects > i_pDynamicObjects ); - virtual void OnDisconnect(); - virtual ~IViconCGStreamClientCallback(); -}; - -inline void IViconCGStreamClientCallback::OnConnect() -{ -} - - -inline void IViconCGStreamClientCallback::OnStaticObjects( std::shared_ptr< const VStaticObjects > i_pStaticObjects ) -{ -} - - -inline void IViconCGStreamClientCallback::OnDynamicObjects( std::shared_ptr< const VDynamicObjects > i_pDynamicObjects ) -{ -} - -inline void IViconCGStreamClientCallback::OnDisconnect() -{ -} - -inline IViconCGStreamClientCallback::~IViconCGStreamClientCallback() -{ -} diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClient/ViconCGStreamBayer.cpp b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClient/ViconCGStreamBayer.cpp deleted file mode 100644 index 640b52bde..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClient/ViconCGStreamBayer.cpp +++ /dev/null @@ -1,250 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#include "ViconCGStreamBayer.h" - -#include -#include - -// Bayer pattern: -// -// R G R G R G R G -// G B G B G B G B -// R G R G R G R G -// G B G B G B G B -// R G R G R G R G -// G B G B G B G B -// R G R G R G R G -// G B G B G B G B - -void VViconCGStreamBayer::BayerGBToBGR( unsigned int i_Width, unsigned int i_Height, const unsigned char * i_pBayerData, unsigned char * o_pBGRData ) -{ - assert( ( i_Width % 2 ) == 0 ); - assert( ( i_Height % 2 ) == 0 ); - - std::memset( o_pBGRData, 0, i_Width * 3 ); - - const unsigned char * pLineAbove = i_pBayerData; - const unsigned char * pLine = i_pBayerData + i_Width; - unsigned char * pBGR = o_pBGRData + i_Width * 3; - - for( unsigned int Y = 1; Y < i_Height; Y++ ) - { - if( ( Y & 1 ) == 0 ) - { - for( unsigned int X = 0; ( X + 1 ) < i_Width; X += 2 ) - { - // Second line (odd lines): red, green - pBGR[ 0 ] = pLineAbove[ 1 ]; - pBGR[ 1 ] = ( pLineAbove[ 0 ] + pLine[ 1 ] ) / 2; - pBGR[ 2 ] = pLine[ 0 ]; - - pLine++; - pLineAbove++; - pBGR += 3; - - pBGR[ 0 ] = pLineAbove[ 0 ]; - pBGR[ 1 ] = ( pLineAbove[ 1 ] + pLine[ 0 ] ) / 2; - pBGR[ 2 ] = pLine[ 1 ]; - - pLine++; - pLineAbove++; - pBGR += 3; - } - } - else - { - for( unsigned int X = 0; ( X + 1 ) < i_Width; X += 2 ) - { - // First line (even lines): green, blue - pBGR[ 0 ] = pLine[ 1 ]; - pBGR[ 1 ] = ( pLine[ 0 ] + pLineAbove[ 1 ] ) / 2; - pBGR[ 2 ] = pLineAbove[ 0 ]; - - pLine++; - pLineAbove++; - pBGR += 3; - - pBGR[ 0 ] = pLine[ 0 ]; - pBGR[ 1 ] = ( pLine[ 1 ] + pLineAbove[ 0 ] ) / 2; - pBGR[ 2 ] = pLineAbove[ 1 ]; - - pLine++; - pLineAbove++; - pBGR += 3; - } - } - - // Clear right-hand edge. - *( pBGR - 1 ) = 0; - *( pBGR - 2 ) = 0; - *( pBGR - 3 ) = 0; - } -} - -// Bayer pattern: -// -// G R G R G R G -// B G B G B G B -// G R G R G R G -// B G B G B G B -// G R G R G R G -// B G B G B G B -// G R G R G R G -// B G B G B G B - -void VViconCGStreamBayer::BayerBGToBGR( unsigned int i_Width, unsigned int i_Height, const unsigned char * i_pBayerData, unsigned char * o_pBGRData ) -{ - assert( ( i_Width % 2 ) == 0 ); - assert( ( i_Height % 2 ) == 0 ); - - std::memset( o_pBGRData, 0, i_Width * 3 ); - - const unsigned char * pLineAbove = i_pBayerData; - const unsigned char * pLine = i_pBayerData + i_Width; - unsigned char * pBGR = o_pBGRData + i_Width * 3; - - for( unsigned int Y = 1; Y < i_Height; Y++ ) - { - if( ( Y & 1 ) == 0 ) - { - for( unsigned int X = 0; ( X + 1 ) < i_Width; X += 2 ) - { - // Second line (odd lines): green, red - pBGR[ 0 ] = pLineAbove[ 0 ]; - pBGR[ 1 ] = ( pLineAbove[ 1 ] + pLine[ 0 ] ) / 2; - pBGR[ 2 ] = pLine[ 1 ]; - - pLine++; - pLineAbove++; - pBGR += 3; - - pBGR[ 0 ] = pLineAbove[ 1 ]; - pBGR[ 1 ] = ( pLineAbove[ 0 ] + pLine[ 1 ] ) / 2; - pBGR[ 2 ] = pLine[ 0 ]; - - pLine++; - pLineAbove++; - pBGR += 3; - } - } - else - { - for( unsigned int X = 0; ( X + 1 ) < i_Width; X += 2 ) - { - // First line (odd lines): blue, green - pBGR[ 0 ] = pLine[ 0 ]; - pBGR[ 1 ] = ( pLine[ 1 ] + pLineAbove[ 0 ] ) / 2; - pBGR[ 2 ] = pLineAbove[ 1 ]; - - pLine++; - pLineAbove++; - pBGR += 3; - - pBGR[ 0 ] = pLine[ 1 ]; - pBGR[ 1 ] = ( pLine[ 0 ] + pLineAbove[ 1 ] ) / 2; - pBGR[ 2 ] = pLineAbove[ 0 ]; - - pLine++; - pLineAbove++; - pBGR += 3; - } - } - - // Clear right-hand edge. - *( pBGR - 1 ) = 0; - *( pBGR - 2 ) = 0; - *( pBGR - 3 ) = 0; - } -} - -// Bayer pattern: -// -// G B G B G B G -// R G R G R G R - -void VViconCGStreamBayer::BayerRGToBGR( unsigned int i_Width, unsigned int i_Height, const unsigned char * i_pBayerData, unsigned char * o_pBGRData ) -{ - assert( ( i_Width % 2 ) == 0 ); - assert( ( i_Height % 2 ) == 0 ); - - std::memset( o_pBGRData, 0, i_Width * 3 ); - - const unsigned char * pLineAbove = i_pBayerData; - const unsigned char * pLine = i_pBayerData + i_Width; - unsigned char * pBGR = o_pBGRData + i_Width * 3; - - for( unsigned int Y = 1; Y < i_Height; Y++ ) - { - if( ( Y & 1 ) == 0 ) - { - for( unsigned int X = 0; ( X + 1 ) < i_Width; X += 2 ) - { - // Second line (odd lines): green, blue - pBGR[ 0 ] = pLine[ 1 ]; - pBGR[ 1 ] = ( pLineAbove[ 1 ] + pLine[ 0 ] ) / 2; - pBGR[ 2 ] = pLineAbove[ 0 ]; - - pLine++; - pLineAbove++; - pBGR += 3; - - pBGR[ 0 ] = pLine[ 0 ]; - pBGR[ 1 ] = ( pLineAbove[ 0 ] + pLine[ 1 ] ) / 2; - pBGR[ 2 ] = pLineAbove[ 1 ]; - - pLine++; - pLineAbove++; - pBGR += 3; - } - } - else - { - for( unsigned int X = 0; ( X + 1 ) < i_Width; X += 2 ) - { - // First line (odd lines): red, green - pBGR[ 0 ] = pLineAbove[ 1 ]; - pBGR[ 1 ] = ( pLine[ 1 ] + pLineAbove[ 0 ] ) / 2; - pBGR[ 2 ] = pLine[ 0 ]; - - pLine++; - pLineAbove++; - pBGR += 3; - - pBGR[ 0 ] = pLineAbove[ 0 ]; - pBGR[ 1 ] = ( pLine[ 0 ] + pLineAbove[ 1 ] ) / 2; - pBGR[ 2 ] = pLine[ 1 ]; - - pLine++; - pLineAbove++; - pBGR += 3; - } - } - - // Clear right-hand edge. - *( pBGR - 1 ) = 0; - *( pBGR - 2 ) = 0; - *( pBGR - 3 ) = 0; - } -} diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClient/ViconCGStreamBayer.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClient/ViconCGStreamBayer.h deleted file mode 100644 index e835b860a..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClient/ViconCGStreamBayer.h +++ /dev/null @@ -1,33 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -class VViconCGStreamBayer -{ -public: - static void BayerGBToBGR( unsigned int i_Width, unsigned int i_Height, const unsigned char * i_pBayerData, unsigned char * o_pBGRData ); - static void BayerBGToBGR( unsigned int i_Width, unsigned int i_Height, const unsigned char * i_pBayerData, unsigned char * o_pBGRData ); - static void BayerRGToBGR( unsigned int i_Width, unsigned int i_Height, const unsigned char * i_pBayerData, unsigned char * o_pBGRData ); -}; diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClient/ViconCGStreamClient.cpp b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClient/ViconCGStreamClient.cpp deleted file mode 100644 index fb9c15254..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClient/ViconCGStreamClient.cpp +++ /dev/null @@ -1,1341 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#ifdef _WIN32 - #include -#endif - -#include "ViconCGStreamClient.h" -#include "IViconCGStreamClientCallback.h" - -#include "ViconCGStreamBayer.h" -#include "CGStreamReaderWriter.h" - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -namespace -{ - // Do delay. - const bool s_bNoDelay = true; - - // Send buffer size. - const size_t s_SocketBufferSize = 4 * 1024 * 1024; - - const size_t s_GetFrameMaxSize = 20; -} - -namespace bc = boost::chrono; -typedef bc::high_resolution_clock hrc; - -//------------------------------------------------------------------------------------------------- - -ViconCGStream::VCentroids & VDynamicObjects::AddCentroids() -{ - m_Centroids.push_back( ViconCGStream::VCentroids() ); - return m_Centroids.back(); -} - -ViconCGStream::VCentroidTracks & VDynamicObjects::AddCentroidTracks() -{ - m_CentroidTracks.push_back( ViconCGStream::VCentroidTracks() ); - return m_CentroidTracks.back(); -} - -ViconCGStream::VCentroidWeights & VDynamicObjects::AddCentroidWeights() -{ - m_CentroidWeights.push_back( ViconCGStream::VCentroidWeights() ); - return m_CentroidWeights.back(); -} - -ViconCGStream::VLocalSegments & VDynamicObjects::AddLocalSegments() -{ - m_LocalSegments.push_back( ViconCGStream::VLocalSegments() ); - return m_LocalSegments.back(); -} - -ViconCGStream::VGlobalSegments & VDynamicObjects::AddGlobalSegments() -{ - m_GlobalSegments.push_back( ViconCGStream::VGlobalSegments() ); - return m_GlobalSegments.back(); -} - -ViconCGStream::VGreyscaleBlobs & VDynamicObjects::AddGreyscaleBlobs() -{ - m_GreyscaleBlobs.push_back( ViconCGStream::VGreyscaleBlobs() ); - return m_GreyscaleBlobs.back(); -} - -ViconCGStream::VEdgePairs & VDynamicObjects::AddEdgePairs() -{ - m_EdgePairs.push_back( ViconCGStream::VEdgePairs() ); - return m_EdgePairs.back(); -} - -ViconCGStream::VForceFrame & VDynamicObjects::AddForceFrame() -{ - m_ForceFrames.push_back( ViconCGStream::VForceFrame() ); - return m_ForceFrames.back(); -} - -ViconCGStream::VMomentFrame & VDynamicObjects::AddMomentFrame() -{ - m_MomentFrames.push_back( ViconCGStream::VMomentFrame() ); - return m_MomentFrames.back(); -} - -ViconCGStream::VCentreOfPressureFrame & VDynamicObjects::AddCentreOfPressureFrame() -{ - m_CentreOfPressureFrames.push_back( ViconCGStream::VCentreOfPressureFrame() ); - return m_CentreOfPressureFrames.back(); -} - -ViconCGStream::VVoltageFrame & VDynamicObjects::AddVoltageFrame() -{ - m_VoltageFrames.push_back( ViconCGStream::VVoltageFrame() ); - return m_VoltageFrames.back(); -} - -ViconCGStream::VCameraWand2d & VDynamicObjects::AddCameraWand2d() -{ - m_CameraWand2d.push_back( ViconCGStream::VCameraWand2d() ); - return m_CameraWand2d.back(); -} - -ViconCGStream::VCameraWand3d & VDynamicObjects::AddCameraWand3d() -{ - m_CameraWand3d.push_back( ViconCGStream::VCameraWand3d() ); - return m_CameraWand3d.back(); -} - -ViconCGStream::VEyeTrackerFrame & VDynamicObjects::AddEyeTrackerFrame() -{ - m_EyeTrackerFrames.push_back( ViconCGStream::VEyeTrackerFrame() ); - return m_EyeTrackerFrames.back(); -} - -ViconCGStream::VVideoFrame & VDynamicObjects::AddVideoFrame() -{ - m_VideoFrames.push_back( std::shared_ptr< ViconCGStream::VVideoFrame >( new ViconCGStream::VVideoFrame() ) ); - return *m_VideoFrames.back(); -} - -//------------------------------------------------------------------------------------------------- - -ViconCGStream::VCameraInfo & VStaticObjects::AddCameraInfo() -{ - m_CameraInfo.resize( m_CameraInfo.size() + 1 ); - return m_CameraInfo.back(); -} - -ViconCGStream::VCameraCalibrationInfo & VStaticObjects::AddCameraCalibrationInfo() -{ - m_CameraCalibrationInfo.resize( m_CameraCalibrationInfo.size() + 1 ); - return m_CameraCalibrationInfo.back(); -} - -ViconCGStream::VCameraCalibrationHealth & VStaticObjects::ResetCameraCalibrationHealth() -{ - m_pCameraCalibrationHealth.reset( new ViconCGStream::VCameraCalibrationHealth ); - return *m_pCameraCalibrationHealth; -} - -ViconCGStream::VSubjectInfo & VStaticObjects::AddSubjectInfo() -{ - m_SubjectInfo.resize( m_SubjectInfo.size() + 1 ); - return m_SubjectInfo.back(); -} - -ViconCGStream::VSubjectTopology & VStaticObjects::AddSubjectTopology() -{ - m_SubjectTopology.resize( m_SubjectTopology.size() + 1 ); - return m_SubjectTopology.back(); -} - -ViconCGStream::VSubjectHealth & VStaticObjects::AddSubjectHealth() -{ - m_SubjectHealth.resize( m_SubjectHealth.size() + 1 ); - return m_SubjectHealth.back(); -} - -ViconCGStream::VObjectQuality & VStaticObjects::AddObjectQuality() -{ - m_ObjectQuality.resize( m_ObjectQuality.size() + 1 ); - return m_ObjectQuality.back(); -} - -ViconCGStream::VDeviceInfo & VStaticObjects::AddDeviceInfo() -{ - m_DeviceInfo.resize( m_DeviceInfo.size() + 1 ); - return m_DeviceInfo.back(); -} - -ViconCGStream::VDeviceInfoExtra & VStaticObjects::AddDeviceInfoExtra() -{ - m_DeviceInfoExtra.resize( m_DeviceInfoExtra.size() + 1 ); - return m_DeviceInfoExtra.back(); -} - -ViconCGStream::VChannelInfo & VStaticObjects::AddChannelInfo() -{ - m_ChannelInfo.resize( m_ChannelInfo.size() + 1 ); - return m_ChannelInfo.back(); -} - -ViconCGStream::VChannelInfoExtra & VStaticObjects::AddChannelInfoExtra() -{ - m_ChannelInfoExtra.resize( m_ChannelInfoExtra.size() + 1 ); - return m_ChannelInfoExtra.back(); -} - -ViconCGStream::VForcePlateInfo & VStaticObjects::AddForcePlateInfo() -{ - m_ForcePlateInfo.resize( m_ForcePlateInfo.size() + 1 ); - return m_ForcePlateInfo.back(); -} - -ViconCGStream::VEyeTrackerInfo & VStaticObjects::AddEyeTrackerInfo() -{ - m_EyeTrackerInfo.resize( m_EyeTrackerInfo.size() + 1 ); - return m_EyeTrackerInfo.back(); -} - -void VStaticObjects::BuildMaps() -{ - m_CameraMap.clear(); - m_CameraCalibrationMap.clear(); - m_SubjectMap.clear(); - m_SegmentMap.clear(); - m_DeviceMap.clear(); - m_ChannelMap.clear(); - - for( unsigned int CameraIndex = 0; CameraIndex != m_CameraInfo.size(); ++CameraIndex ) - { - ViconCGStream::VCameraInfo & rCameraInfo = m_CameraInfo[ CameraIndex ]; - m_CameraMap.insert( std::make_pair( rCameraInfo.m_CameraID, CameraIndex ) ); - } - - for( unsigned int CameraCalibrationIndex = 0; CameraCalibrationIndex != m_CameraCalibrationInfo.size(); ++CameraCalibrationIndex ) - { - ViconCGStream::VCameraCalibrationInfo & rCameraCalibrationInfo = m_CameraCalibrationInfo[ CameraCalibrationIndex ]; - m_CameraCalibrationMap.insert( std::make_pair( rCameraCalibrationInfo.m_CameraID, CameraCalibrationIndex ) ); - } - - for( unsigned int SubjectIndex = 0; SubjectIndex != m_SubjectInfo.size(); ++SubjectIndex ) - { - ViconCGStream::VSubjectInfo & rSubjectInfo = m_SubjectInfo[ SubjectIndex ]; - - m_SubjectMap.insert( std::make_pair( rSubjectInfo.m_SubjectID, SubjectIndex ) ); - - for( unsigned int SegmentIndex = 0; SegmentIndex != rSubjectInfo.m_Segments.size(); ++SegmentIndex ) - { - TNestedIDPair SegmentID( rSubjectInfo.m_SubjectID, rSubjectInfo.m_Segments[ SegmentIndex ].m_SegmentID ); - m_SegmentMap.insert( std::make_pair( SegmentID, SegmentIndex ) ); - } - } - - for( unsigned int DeviceIndex = 0; DeviceIndex != m_DeviceInfo.size(); ++DeviceIndex ) - { - ViconCGStream::VDeviceInfo & rDeviceInfo = m_DeviceInfo[ DeviceIndex ]; - - m_DeviceMap.insert( std::make_pair( rDeviceInfo.m_DeviceID, DeviceIndex ) ); - } - - for( unsigned int ChannelIndex = 0; ChannelIndex != m_ChannelInfo.size(); ++ChannelIndex ) - { - ViconCGStream::VChannelInfo & rChannelInfo = m_ChannelInfo[ ChannelIndex ]; - - TNestedIDPair ChannelID( rChannelInfo.m_DeviceID, rChannelInfo.m_ChannelID ); - m_ChannelMap.insert( std::make_pair( ChannelID, ChannelIndex ) ); - } -} - -//------------------------------------------------------------------------------------------------- - -VViconCGStreamClient::VViconCGStreamClient( std::weak_ptr< IViconCGStreamClientCallback > i_pCallback ) -: m_pCallback( i_pCallback ) -, m_bEnumsChanged( false ) -, m_bStreamingChanged( true ) -, m_bStreaming( false ) -, m_bHapticChanged( false ) -, m_bFilterChanged( false ) -, m_VideoHint( EPassThrough ) -{ - m_pSocket.reset( new boost::asio::ip::tcp::socket( m_Service ) ); -} - -VViconCGStreamClient::~VViconCGStreamClient() -{ - Disconnect(); -} - -void VViconCGStreamClient::Connect( const std::string & i_rHost, unsigned short i_Port ) -{ - if( m_pMulticastSocket ) - { - return ; - } - - boost::asio::ip::tcp::resolver Resolver( m_Service ); - boost::asio::ip::tcp::resolver::query Query( i_rHost, "" ); - - boost::system::error_code Error; - boost::asio::ip::tcp::resolver::iterator It = Resolver.resolve( Query, Error ); - boost::asio::ip::tcp::resolver::iterator End; - - if( Error ) - { - OnDisconnect(); - return; - } - - bool bConnected = false; - for( ; It != End; ++It ) - { - Error = boost::system::error_code(); - boost::asio::ip::tcp::endpoint EndPoint( *It ); - - // Currently we only handle IPv4 - // This has to be explicitly handled, otherwise the socket can bind to a v6 endpoint and then fail - // to connect to a v4 endpoint - which is a bit rubbish. - if( ! EndPoint.address().is_v4() ) - { - continue; - } - - EndPoint.port( i_Port ); - - m_pSocket->open( EndPoint.protocol(), Error ); - if( ! Error ) - { - m_pSocket->set_option( boost::asio::socket_base::reuse_address( true ), Error ); - } - if( ! Error ) - { - m_pSocket->set_option( boost::asio::ip::tcp::no_delay( s_bNoDelay ), Error ); - } - if( ! Error ) - { - boost::system::error_code LocalError; - m_pSocket->set_option( boost::asio::socket_base::receive_buffer_size( s_SocketBufferSize ), LocalError ); - // On Mac OS X, setting the buffer size resulted in an error "no_buffer_space" -#if !defined(__APPLE__) - Error = LocalError; -#endif - } - if( ! Error ) - { - m_pSocket->connect( EndPoint, Error ); - } - - if( Error ) - { - m_pSocket->close(); - std::stringstream Strm; - Strm << Error; - std::string ErrorText = Strm.str(); - // std::cerr << Error << std::endl; - } - if( ! Error ) - { - bConnected = true; - break; - } - } - - if( bConnected ) - { - OnConnect(); - } - else - { - OnDisconnect(); - return; - } - - m_pClientThread.reset(new boost::thread(boost::bind(&VViconCGStreamClient::ClientThread, this))); -} - -void VViconCGStreamClient::Disconnect() -{ - boost::system::error_code DontCareError; - m_pSocket->shutdown( boost::asio::ip::tcp::socket::shutdown_both, DontCareError ); - m_pSocket->close(); - if( m_pMulticastSocket ) - { - boost::system::error_code DontCareError2; - m_pMulticastSocket->shutdown( boost::asio::ip::tcp::socket::shutdown_both, DontCareError2 ); - m_pMulticastSocket->close(); - } - - if( m_pClientThread ) - { - m_pClientThread->join(); - m_pClientThread.reset(); - } - m_pMulticastSocket.reset(); -} - -void VViconCGStreamClient::ReceiveMulticastData( std::string i_MulticastIPAddress, std::string i_LocalIPAddress, unsigned short i_Port ) -{ - boost::asio::ip::address_v4 MulticastAddress = FirstV4AddressFromString( i_MulticastIPAddress ); - boost::asio::ip::address_v4 LocalAddress = FirstV4AddressFromString( i_LocalIPAddress ); - boost::asio::ip::udp::endpoint LocalEndpoint( LocalAddress, i_Port ); - - boost::system::error_code Error; - - if( m_pSocket->is_open() ) - { - Disconnect(); - } - - if( !MulticastAddress.is_multicast() && ( MulticastAddress.to_ulong() != 0xFFFFFFFF ) ) - { - OnDisconnect(); - return ; - } - - std::shared_ptr< boost::asio::ip::udp::socket > pMulticastSocket( - new boost::asio::ip::udp::socket( m_Service, LocalEndpoint.protocol() ) ); - if( Error ) - { - OnDisconnect(); - return ; - } - pMulticastSocket->set_option( boost::asio::socket_base::reuse_address( true ), Error ); - if( Error ) - { - OnDisconnect(); - return ; - } - pMulticastSocket->set_option( boost::asio::socket_base::receive_buffer_size( 128 * 1024 ), Error ); - if( Error ) - { - OnDisconnect(); - return ; - } -#ifdef WIN32 - pMulticastSocket->bind( LocalEndpoint, Error ); -#else - if( MulticastAddress.is_multicast() ) - { - boost::asio::ip::udp::endpoint MulticastEndpoint( MulticastAddress, i_Port ); - pMulticastSocket->bind( MulticastEndpoint, Error ); - } - else - { - boost::asio::ip::udp::endpoint BroadcastEndpoint( MulticastAddress, i_Port ); - pMulticastSocket->bind( BroadcastEndpoint, Error ); - } -#endif - if( Error ) - { - OnDisconnect(); - return ; - } - if( MulticastAddress.is_multicast() ) - { - pMulticastSocket->set_option( boost::asio::ip::multicast::join_group( MulticastAddress, LocalAddress ), Error ); - } - if( Error ) - { - OnDisconnect(); - return ; - } - - m_pMulticastSocket = pMulticastSocket; - - m_pClientThread.reset( new boost::thread( boost::bind( &VViconCGStreamClient::ClientThread, this ) ) ); -} - -void VViconCGStreamClient::StopReceivingMulticastData( ) -{ - Disconnect(); -} - -void VViconCGStreamClient::SetStreaming( bool i_bStreaming ) -{ - boost::recursive_mutex::scoped_lock Lock( m_Mutex ); - m_bStreaming = i_bStreaming; - m_bStreamingChanged = true; -} - -void VViconCGStreamClient::SetFilter( const ViconCGStream::VFilter & i_rFilter ) -{ - boost::recursive_mutex::scoped_lock Lock( m_Mutex ); - m_Filter = i_rFilter; - m_bFilterChanged = true; -} - -void VViconCGStreamClient::SetRequiredObjects( std::set< ViconCGStreamType::Enum > & i_rRequiredObjects ) -{ - boost::recursive_mutex::scoped_lock Lock( m_Mutex ); - m_RequiredObjects.m_Enums = i_rRequiredObjects; - m_RequiredObjects.m_Enums.insert( ViconCGStreamEnum::Contents ); - m_bEnumsChanged = true; -} - -void VViconCGStreamClient::SetApexDeviceFeedback( const std::set< unsigned int > &i_rDeviceList ) -{ - if( i_rDeviceList != m_OnDeviceList ) - { - m_bHapticChanged = true; - m_OnDeviceList = i_rDeviceList; - } -} - -void VViconCGStreamClient::SetServerToTransmitMulticast( std::string i_MulticastIPAddress, std::string i_ServerIPAddress, unsigned short i_Port ) -{ - boost::recursive_mutex::scoped_lock Lock( m_Mutex ); - - VCGStreamReaderWriter ReaderWriter( m_pSocket ); - { - ViconCGStreamIO::VScopedWriter Objects( ReaderWriter ); - - ViconCGStream::VStartMulticastSender RequestMulticast; - - boost::asio::ip::address_v4 MulticastAddress = FirstV4AddressFromString( i_MulticastIPAddress ); - boost::asio::ip::address_v4 ServerAddress = FirstV4AddressFromString( i_ServerIPAddress ); - - RequestMulticast.m_MulticastIpAddress = MulticastAddress.to_ulong(); - RequestMulticast.m_SourceIpAddress = ServerAddress.to_ulong(); - RequestMulticast.m_Port = i_Port; - - Objects.Write( RequestMulticast ); - } - ReaderWriter.Flush(); -} - -void VViconCGStreamClient::StopMulticastTransmission( ) -{ - boost::recursive_mutex::scoped_lock Lock( m_Mutex ); - - VCGStreamReaderWriter ReaderWriter( m_pSocket ); - { - ViconCGStreamIO::VScopedWriter Objects( ReaderWriter ); - - ViconCGStream::VStopMulticastSender StopMulticast; - - Objects.Write( StopMulticast ); - } - ReaderWriter.Flush(); -} - -void VViconCGStreamClient::SetVideoHint( EVideoHint i_VideoHint ) -{ - boost::recursive_mutex::scoped_lock Lock( m_Mutex ); - m_VideoHint = i_VideoHint; -} - -static void Intersect( ViconCGStream::VObjectEnums & i_rServer, ViconCGStream::VObjectEnums & i_rClient, ViconCGStream::VObjectEnums & i_rOutput ) -{ - std::set< ViconCGStreamType::Enum > & i_rA = i_rServer.m_Enums; - std::set< ViconCGStreamType::Enum > & i_rB = i_rClient.m_Enums; - std::set< ViconCGStreamType::Enum > & i_rC = i_rOutput.m_Enums; - i_rC.clear(); - - std::set< ViconCGStreamType::Enum >::iterator It = i_rA.begin(); - std::set< ViconCGStreamType::Enum >::iterator End = i_rA.end(); - for( ; It != End; ++It ) - { - if( i_rB.find( *It ) != i_rB.end() ) - { - i_rC.insert( *It ); - } - } -} - -bool VViconCGStreamClient::NetworkLatency( double & o_rLatency ) const -{ - if (m_GetFrameTimes.size() < s_GetFrameMaxSize) - { - return false; - } - - o_rLatency = std::accumulate(m_GetFrameTimes.begin(), m_GetFrameTimes.end(), 0.0) / m_GetFrameTimes.size() ; - - return true; -} - -void VViconCGStreamClient::ClientThread() -{ - m_pStaticObjects.reset(); - m_pDynamicObjects.reset(); - - // We are timing our get_frame loop to determine network latency - size_t FrameTimeIndex = 0; - m_GetFrameTimes.resize(s_GetFrameMaxSize); - - if( m_pMulticastSocket ) - { - // Multicast receive only - VCGStreamReaderWriter ReaderWriter( m_pMulticastSocket ); - - - for( ;; ) - { - hrc::time_point Before = hrc::now(); - - if( !ReadObjects( ReaderWriter ) ) - { - break; - } - - hrc::time_point After = hrc::now(); - bc::duration< double, boost::milli > TimeMS = ( After - Before ) / 2 ; - m_GetFrameTimes[ FrameTimeIndex++ % s_GetFrameMaxSize ] = TimeMS.count(); - } - } - else - { - // TCP read and write - VCGStreamReaderWriter ReaderWriter( m_pSocket ); - if( !ReadObjectEnums( ReaderWriter ) ) - { - OnDisconnect(); - return; - } - - for( ;; ) - { - - hrc::time_point Before = hrc::now(); - - if( !WriteObjects( ReaderWriter ) ) - { - break; - } - if( !ReadObjects( ReaderWriter ) ) - { - break; - } - hrc::time_point After = hrc::now(); - bc::duration< double, boost::milli > TimeMS = (After - Before) / 2; - m_GetFrameTimes[FrameTimeIndex++ % s_GetFrameMaxSize] = TimeMS.count(); - } - - OnDisconnect(); - } -} - -bool VViconCGStreamClient::ReadObjectEnums( VCGStreamReaderWriter & i_rReaderWriter ) -{ - if( !i_rReaderWriter.Fill() ) - { - return false; - } - - ViconCGStreamIO::VScopedReader Objects( i_rReaderWriter ); - if( Objects.Enum() != ViconCGStreamEnum::Objects ) - { - return false; - } - - while( Objects.Ok() ) - { - ViconCGStreamIO::VScopedReader Object( i_rReaderWriter ); - - if( Object.Enum() == ViconCGStreamEnum::ObjectEnums ) - { - if( !Object.Read( m_ServerObjects ) ) - { - return false; - } - - m_bEnumsChanged = true; - return true; - } - } - - return false; -} - -bool VViconCGStreamClient::WriteObjects( VCGStreamReaderWriter & i_rReaderWriter ) -{ - boost::recursive_mutex::scoped_lock Lock( m_Mutex ); - bool bWriteObjects = m_bStreamingChanged || m_bEnumsChanged || !m_bStreaming || m_bHapticChanged; - - if( bWriteObjects ) - { - i_rReaderWriter.Clear(); - ViconCGStreamIO::VScopedWriter Objects( i_rReaderWriter ); - - if( m_bEnumsChanged ) - { - ViconCGStream::VObjectEnums Output; - Intersect( m_ServerObjects, m_RequiredObjects, Output ); - Objects.Write( Output ); - } - - if( !m_bStreaming || m_bStreamingChanged ) - { - ViconCGStream::VRequestFrame RequestFrame; - RequestFrame.m_bStreaming = m_bStreaming; - Objects.Write( RequestFrame ); - } - - // write haptic feedback - if( m_bHapticChanged ) - { - ViconCGStream::VApexHaptics HapticFeedback; - //set haptic on and off devices - HapticFeedback.m_OnDevicesList = m_OnDeviceList; - Objects.Write( HapticFeedback ); - m_bHapticChanged = false; - } - - if ( m_bFilterChanged ) - { - Objects.Write( m_Filter ); - m_bFilterChanged = false; - } - } - else - { - return true; - } - - m_bEnumsChanged = false; - m_bStreamingChanged = false; - - return i_rReaderWriter.Flush(); -} - -void VViconCGStreamClient::CopyObjects( const ViconCGStream::VContents & i_rContents, const VStaticObjects & i_rStaticObjects, VStaticObjects & o_rStaticObjects ) const -{ - typedef std::set< ViconCGStreamType::Enum > TEnums; - - TEnums::const_iterator It = i_rContents.m_EnumsUnchanged.begin(); - TEnums::const_iterator End = i_rContents.m_EnumsUnchanged.end(); - - for( ; It != End; ++It ) - { - switch( *It ) - { - case ViconCGStreamEnum::ApplicationInfo: - o_rStaticObjects.m_ApplicationInfo = i_rStaticObjects.m_ApplicationInfo; - break; - case ViconCGStreamEnum::StreamInfo: - o_rStaticObjects.m_StreamInfo = i_rStaticObjects.m_StreamInfo; - break; - case ViconCGStreamEnum::CameraInfo: - o_rStaticObjects.m_CameraInfo = i_rStaticObjects.m_CameraInfo; - break; - case ViconCGStreamEnum::CameraCalibrationInfo: - o_rStaticObjects.m_CameraCalibrationInfo = i_rStaticObjects.m_CameraCalibrationInfo; - break; - case ViconCGStreamEnum::CameraCalibrationHealth: - o_rStaticObjects.m_pCameraCalibrationHealth = i_rStaticObjects.m_pCameraCalibrationHealth; - break; - case ViconCGStreamEnum::SubjectInfo: - o_rStaticObjects.m_SubjectInfo = i_rStaticObjects.m_SubjectInfo; - break; - case ViconCGStreamEnum::SubjectTopology: - o_rStaticObjects.m_SubjectTopology = i_rStaticObjects.m_SubjectTopology; - break; - case ViconCGStreamEnum::SubjectHealth: - o_rStaticObjects.m_SubjectHealth = i_rStaticObjects.m_SubjectHealth; - break; - case ViconCGStreamEnum::ObjectQuality: - o_rStaticObjects.m_ObjectQuality = i_rStaticObjects.m_ObjectQuality; - break; - case ViconCGStreamEnum::DeviceInfo: - o_rStaticObjects.m_DeviceInfo = i_rStaticObjects.m_DeviceInfo; - break; - case ViconCGStreamEnum::DeviceInfoExtra: - o_rStaticObjects.m_DeviceInfoExtra = i_rStaticObjects.m_DeviceInfoExtra; - break; - case ViconCGStreamEnum::ChannelInfo: - o_rStaticObjects.m_ChannelInfo = i_rStaticObjects.m_ChannelInfo; - break; - case ViconCGStreamEnum::ChannelInfoExtra: - o_rStaticObjects.m_ChannelInfoExtra = i_rStaticObjects.m_ChannelInfoExtra; - break; - case ViconCGStreamEnum::ForcePlateInfo: - o_rStaticObjects.m_ForcePlateInfo = i_rStaticObjects.m_ForcePlateInfo; - break; - case ViconCGStreamEnum::EyeTrackerInfo: - o_rStaticObjects.m_EyeTrackerInfo = i_rStaticObjects.m_EyeTrackerInfo; - break; - } - } -} - -void VViconCGStreamClient::CopyObjects( const ViconCGStream::VContents & i_rContents, const VDynamicObjects & i_rDynamicObjects, VDynamicObjects & o_rDynamicObjects ) const -{ - typedef std::set< ViconCGStreamType::Enum > TEnums; - - TEnums::const_iterator It = i_rContents.m_EnumsUnchanged.begin(); - TEnums::const_iterator End = i_rContents.m_EnumsUnchanged.end(); - - for( ; It != End; ++It ) - { - switch( *It ) - { - case ViconCGStreamEnum::FrameInfo: - o_rDynamicObjects.m_FrameInfo = i_rDynamicObjects.m_FrameInfo; - break; - case ViconCGStreamEnum::HardwareFrameInfo: - o_rDynamicObjects.m_HardwareFrameInfo = i_rDynamicObjects.m_HardwareFrameInfo; - break; - case ViconCGStreamEnum::Timecode: - o_rDynamicObjects.m_Timecode = i_rDynamicObjects.m_Timecode; - break; - case ViconCGStreamEnum::LatencyInfo: - o_rDynamicObjects.m_LatencyInfo = i_rDynamicObjects.m_LatencyInfo; - break; - case ViconCGStreamEnum::Centroids: - o_rDynamicObjects.m_Centroids = i_rDynamicObjects.m_Centroids; - break; - case ViconCGStreamEnum::CentroidWeights: - o_rDynamicObjects.m_CentroidWeights = i_rDynamicObjects.m_CentroidWeights; - break; - case ViconCGStreamEnum::LabeledRecons: - o_rDynamicObjects.m_LabeledRecons = i_rDynamicObjects.m_LabeledRecons; - break; - case ViconCGStreamEnum::UnlabeledRecons: - o_rDynamicObjects.m_UnlabeledRecons = i_rDynamicObjects.m_UnlabeledRecons; - break; - case ViconCGStreamEnum::LabeledReconRayAssignments: - o_rDynamicObjects.m_LabeledRayAssignments = i_rDynamicObjects.m_LabeledRayAssignments; - break; - case ViconCGStreamEnum::GlobalSegments: - o_rDynamicObjects.m_GlobalSegments = i_rDynamicObjects.m_GlobalSegments; - break; - case ViconCGStreamEnum::LocalSegments: - o_rDynamicObjects.m_LocalSegments = i_rDynamicObjects.m_LocalSegments; - break; - case ViconCGStreamEnum::GreyscaleBlobs: - o_rDynamicObjects.m_GreyscaleBlobs = i_rDynamicObjects.m_GreyscaleBlobs; - break; - case ViconCGStreamEnum::EdgePairs: - o_rDynamicObjects.m_EdgePairs = i_rDynamicObjects.m_EdgePairs; - break; - case ViconCGStreamEnum::CameraWand2d: - o_rDynamicObjects.m_CameraWand2d = i_rDynamicObjects.m_CameraWand2d; - break; - case ViconCGStreamEnum::CameraWand3d: - o_rDynamicObjects.m_CameraWand3d = i_rDynamicObjects.m_CameraWand3d; - break; - case ViconCGStreamEnum::VideoFrame: - o_rDynamicObjects.m_VideoFrames = i_rDynamicObjects.m_VideoFrames; - break; - case ViconCGStreamEnum::EyeTrackerFrame: - o_rDynamicObjects.m_EyeTrackerFrames = i_rDynamicObjects.m_EyeTrackerFrames; - break; - case ViconCGStreamEnum::FrameRateInfo: - o_rDynamicObjects.m_FrameRateInfo = i_rDynamicObjects.m_FrameRateInfo; - break; - } - } -} - -bool VViconCGStreamClient::ReadObjects( VCGStreamReaderWriter & i_rReaderWriter ) -{ - if( !i_rReaderWriter.Fill() ) - { - return false; - } - - ViconCGStreamIO::VScopedReader Objects( i_rReaderWriter ); - if( Objects.Enum() != ViconCGStreamEnum::Objects ) - { - return false; - } - - std::shared_ptr< VStaticObjects > pStaticObjects; - std::shared_ptr< VDynamicObjects > pDynamicObjects; - - bool bContents = false; - ViconCGStream::VContents Contents; - - while( Objects.Ok() ) - { - ViconCGStreamIO::VScopedReader Object( i_rReaderWriter ); - - switch( Object.Enum() ) - { - case ViconCGStreamEnum::Contents: - if( !Object.Read( Contents ) ) - { - return false; - } - - bContents = true; - - break; - case ViconCGStreamEnum::StreamInfo: - if ( !pStaticObjects ) pStaticObjects.reset( new VStaticObjects() ); - if( !Object.Read( pStaticObjects->m_StreamInfo ) ) - { - return false; - } - break; - case ViconCGStreamEnum::ApplicationInfo: - { - ViconCGStream::VApplicationInfo AppInfo; - if( !Object.Read( AppInfo ) ) - { - return false; - } - - if ( !pStaticObjects ) pStaticObjects.reset( new VStaticObjects() ); - pStaticObjects->m_ApplicationInfo = AppInfo; - break; - } - case ViconCGStreamEnum::SubjectInfo: - if ( !pStaticObjects ) pStaticObjects.reset( new VStaticObjects() ); - if( !Object.Read( pStaticObjects->AddSubjectInfo() ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::SubjectTopology: - if ( !pStaticObjects ) pStaticObjects.reset( new VStaticObjects() ); - if( !Object.Read( pStaticObjects->AddSubjectTopology() ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::SubjectHealth: - if ( !pStaticObjects ) pStaticObjects.reset( new VStaticObjects() ); - if( !Object.Read( pStaticObjects->AddSubjectHealth() ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::ObjectQuality: - if ( !pStaticObjects ) pStaticObjects.reset( new VStaticObjects() ); - if( !Object.Read( pStaticObjects->AddObjectQuality() ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::CameraInfo: - if ( !pStaticObjects ) pStaticObjects.reset( new VStaticObjects() ); - if( !Object.Read( pStaticObjects->AddCameraInfo() ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::CameraCalibrationInfo: - if ( !pStaticObjects ) pStaticObjects.reset( new VStaticObjects() ); - if( !Object.Read( pStaticObjects->AddCameraCalibrationInfo() ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::CameraCalibrationHealth: - if ( !pStaticObjects ) pStaticObjects.reset( new VStaticObjects() ); - if( !Object.Read( pStaticObjects->ResetCameraCalibrationHealth() ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::DeviceInfo: - if ( !pStaticObjects ) pStaticObjects.reset( new VStaticObjects() ); - if( !Object.Read( pStaticObjects->AddDeviceInfo() ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::DeviceInfoExtra: - if ( !pStaticObjects ) pStaticObjects.reset( new VStaticObjects() ); - if( !Object.Read( pStaticObjects->AddDeviceInfoExtra() ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::ChannelInfo: - if ( !pStaticObjects ) pStaticObjects.reset( new VStaticObjects() ); - if( !Object.Read( pStaticObjects->AddChannelInfo() ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::ChannelInfoExtra: - if ( !pStaticObjects ) pStaticObjects.reset( new VStaticObjects() ); - if( !Object.Read( pStaticObjects->AddChannelInfoExtra() ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::ForcePlateInfo: - if ( !pStaticObjects ) pStaticObjects.reset( new VStaticObjects() ); - if( !Object.Read( pStaticObjects->AddForcePlateInfo() ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::EyeTrackerInfo: - if ( !pStaticObjects ) pStaticObjects.reset( new VStaticObjects() ); - if( !Object.Read( pStaticObjects->AddEyeTrackerInfo() ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::FrameInfo: - if ( !pDynamicObjects ) pDynamicObjects.reset( new VDynamicObjects() ); - if ( !Object.Read( pDynamicObjects->m_FrameInfo ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::HardwareFrameInfo: - if ( !pDynamicObjects ) pDynamicObjects.reset( new VDynamicObjects() ); - if( !Object.Read( pDynamicObjects->m_HardwareFrameInfo ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::Timecode: - if ( !pDynamicObjects ) pDynamicObjects.reset( new VDynamicObjects() ); - if( !Object.Read( pDynamicObjects->m_Timecode ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::LatencyInfo: - if ( !pDynamicObjects ) pDynamicObjects.reset( new VDynamicObjects() ); - if( !Object.Read( pDynamicObjects->m_LatencyInfo ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::Centroids: - if ( !pDynamicObjects ) pDynamicObjects.reset( new VDynamicObjects() ); - if( !Object.Read( pDynamicObjects->AddCentroids() ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::CentroidTracks: - if ( !pDynamicObjects ) pDynamicObjects.reset( new VDynamicObjects() ); - if( !Object.Read( pDynamicObjects->AddCentroidTracks() ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::CentroidWeights: - if ( !pDynamicObjects ) pDynamicObjects.reset( new VDynamicObjects() ); - if( !Object.Read( pDynamicObjects->AddCentroidWeights() ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::VideoFrame: - { - if ( !pDynamicObjects ) pDynamicObjects.reset( new VDynamicObjects() ); - ViconCGStream::VVideoFrame & rVideoFrame = pDynamicObjects->AddVideoFrame(); - if( !Object.Read( rVideoFrame ) ) - { - return false; - } - - if( m_VideoHint == EDecode ) - { - DecodeVideo( rVideoFrame ); - } - } - break; - case ViconCGStreamEnum::LabeledRecons: - if ( !pDynamicObjects ) pDynamicObjects.reset( new VDynamicObjects() ); - if( !Object.Read( pDynamicObjects->m_LabeledRecons ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::UnlabeledRecons: - if ( !pDynamicObjects ) pDynamicObjects.reset( new VDynamicObjects() ); - if( !Object.Read( pDynamicObjects->m_UnlabeledRecons ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::LabeledReconRayAssignments: - if ( !pDynamicObjects ) pDynamicObjects.reset( new VDynamicObjects() ); - if( !Object.Read( pDynamicObjects->m_LabeledRayAssignments ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::GlobalSegments: - if ( !pDynamicObjects ) pDynamicObjects.reset( new VDynamicObjects() ); - if( !Object.Read( pDynamicObjects->AddGlobalSegments() ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::LocalSegments: - if ( !pDynamicObjects ) pDynamicObjects.reset( new VDynamicObjects() ); - if( !Object.Read( pDynamicObjects->AddLocalSegments() ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::GreyscaleBlobs: - if ( !pDynamicObjects ) pDynamicObjects.reset( new VDynamicObjects() ); - if( !Object.Read( pDynamicObjects->AddGreyscaleBlobs() ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::EdgePairs: - if ( !pDynamicObjects ) pDynamicObjects.reset( new VDynamicObjects() ); - if( !Object.Read( pDynamicObjects->AddEdgePairs() ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::ForceFrame: - if ( !pDynamicObjects ) pDynamicObjects.reset( new VDynamicObjects() ); - if( !Object.Read( pDynamicObjects->AddForceFrame() ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::MomentFrame: - if ( !pDynamicObjects ) pDynamicObjects.reset( new VDynamicObjects() ); - if( !Object.Read( pDynamicObjects->AddMomentFrame() ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::CentreOfPressureFrame: - if ( !pDynamicObjects ) pDynamicObjects.reset( new VDynamicObjects() ); - if( !Object.Read( pDynamicObjects->AddCentreOfPressureFrame() ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::VoltageFrame: - if ( !pDynamicObjects ) pDynamicObjects.reset( new VDynamicObjects() ); - if( !Object.Read( pDynamicObjects->AddVoltageFrame() ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::CameraWand2d: - if ( !pDynamicObjects ) pDynamicObjects.reset( new VDynamicObjects() ); - if( !Object.Read( pDynamicObjects->AddCameraWand2d() ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::CameraWand3d: - if ( !pDynamicObjects ) pDynamicObjects.reset( new VDynamicObjects() ); - if( !Object.Read( pDynamicObjects->AddCameraWand3d() ) ) - { - return false; - } - - break; - case ViconCGStreamEnum::EyeTrackerFrame: - if ( !pDynamicObjects ) pDynamicObjects.reset( new VDynamicObjects() ); - if( !Object.Read( pDynamicObjects->AddEyeTrackerFrame() ) ) - { - return false; - } - - break; - - case ViconCGStreamEnum::FrameRateInfo: - if ( !pDynamicObjects ) pDynamicObjects.reset( new VDynamicObjects() ); - if( !Object.Read( pDynamicObjects->m_FrameRateInfo ) ) - { - return false; - } - - break; - } - } - - if( bContents && m_pStaticObjects && pStaticObjects ) - { - CopyObjects( Contents, *m_pStaticObjects, *pStaticObjects ); - } - - if( bContents && m_pDynamicObjects && pDynamicObjects ) - { - CopyObjects( Contents, *m_pDynamicObjects, *pDynamicObjects ); - } - - if ( pStaticObjects ) - { - m_pStaticObjects = pStaticObjects; - OnStaticObjects( pStaticObjects ); - } - - if ( pDynamicObjects ) - { - m_pDynamicObjects = pDynamicObjects; - OnDynamicObjects( pDynamicObjects ); - } - return true; -} - -//------------------------------------------------------------------------------------------------- - -void VViconCGStreamClient::DecodeVideo( ViconCGStream::VVideoFrame & io_rVideoFrame ) -{ - if( io_rVideoFrame.m_Format == ViconCGStream::VVideoFrame::EBayerGB8 ) - { - m_ScratchVideo.resize( io_rVideoFrame.m_Width * io_rVideoFrame.m_Height * 3 ); - VViconCGStreamBayer::BayerGBToBGR( io_rVideoFrame.m_Width, io_rVideoFrame.m_Height, &io_rVideoFrame.m_VideoData[ 0 ], &m_ScratchVideo[ 0 ] ); - } - else - if( io_rVideoFrame.m_Format == ViconCGStream::VVideoFrame::EBayerBG8 ) - { - m_ScratchVideo.resize( io_rVideoFrame.m_Width * io_rVideoFrame.m_Height * 3 ); - VViconCGStreamBayer::BayerBGToBGR( io_rVideoFrame.m_Width, io_rVideoFrame.m_Height, &io_rVideoFrame.m_VideoData[ 0 ], &m_ScratchVideo[ 0 ] ); - } - else - if( io_rVideoFrame.m_Format == ViconCGStream::VVideoFrame::EBayerRG8 ) - { - m_ScratchVideo.resize( io_rVideoFrame.m_Width * io_rVideoFrame.m_Height * 3 ); - VViconCGStreamBayer::BayerRGToBGR( io_rVideoFrame.m_Width, io_rVideoFrame.m_Height, &io_rVideoFrame.m_VideoData[ 0 ], &m_ScratchVideo[ 0 ] ); - } - else - { - return; - } - - io_rVideoFrame.m_Format = ViconCGStream::VVideoFrame::EBGR888; - io_rVideoFrame.m_VideoData = m_ScratchVideo; -} - -//------------------------------------------------------------------------------------------------- - -void VViconCGStreamClient::OnConnect() const -{ - std::shared_ptr< IViconCGStreamClientCallback > pCallback = m_pCallback.lock(); - if( pCallback ) - { - pCallback->OnConnect(); - } -} - -void VViconCGStreamClient::OnStaticObjects( std::shared_ptr< const VStaticObjects > i_pStaticObjects ) const -{ - std::shared_ptr< IViconCGStreamClientCallback > pCallback = m_pCallback.lock(); - if( pCallback ) - { - pCallback->OnStaticObjects( i_pStaticObjects ); - } -} - -void VViconCGStreamClient::OnDynamicObjects( std::shared_ptr< const VDynamicObjects > i_pDynamicObjects ) const -{ - std::shared_ptr< IViconCGStreamClientCallback > pCallback = m_pCallback.lock(); - if( pCallback ) - { - pCallback->OnDynamicObjects( i_pDynamicObjects ); - } -} - -void VViconCGStreamClient::OnDisconnect() const -{ - std::shared_ptr< IViconCGStreamClientCallback > pCallback = m_pCallback.lock(); - if( pCallback ) - { - pCallback->OnDisconnect(); - } -} - -boost::asio::ip::address_v4 VViconCGStreamClient::FirstV4AddressFromString( const std::string & i_rAddress ) -{ - boost::system::error_code Error; - boost::asio::ip::address_v4 Address = boost::asio::ip::address_v4::from_string( i_rAddress, Error ); - if( ! Error ) - { - return Address; - } - - boost::asio::ip::tcp::resolver Resolver( m_Service ); - boost::asio::ip::tcp::resolver::query Query( i_rAddress, "" ); - - boost::asio::ip::tcp::resolver::iterator It = Resolver.resolve( Query, Error ); - boost::asio::ip::tcp::resolver::iterator End; - - if( ! Error ) - { - for( ; It != End; ++It ) - { - Error = boost::system::error_code(); - boost::asio::ip::tcp::endpoint EndPoint( *It ); - - // Currently we only handle IPv4 - if( EndPoint.address().is_v4() ) - { - return EndPoint.address().to_v4(); - } - } - } - - return boost::asio::ip::address_v4(); -} diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClient/ViconCGStreamClient.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClient/ViconCGStreamClient.h deleted file mode 100644 index a9b40af89..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClient/ViconCGStreamClient.h +++ /dev/null @@ -1,270 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -#include "IViconCGStreamClientCallback.h" - -#include - -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -class VCGStreamReaderWriter; - -//------------------------------------------------------------------------------------------------- - -class VStaticObjects -{ -public: - - ViconCGStream::VStreamInfo m_StreamInfo; - boost::optional m_ApplicationInfo; - - typedef std::vector< ViconCGStream::VCameraInfo > TCameraInfo; - typedef std::vector< ViconCGStream::VCameraCalibrationInfo > TCameraCalibrationInfo; - typedef std::shared_ptr< ViconCGStream::VCameraCalibrationHealth > TCameraCalibrationHealth; - typedef std::vector< ViconCGStream::VSubjectInfo > TSubjectInfo; - typedef std::vector< ViconCGStream::VSubjectTopology > TSubjectTopology; - typedef std::vector< ViconCGStream::VSubjectHealth > TSubjectHealth; - typedef std::vector< ViconCGStream::VObjectQuality > TObjectQuality; - typedef std::vector< ViconCGStream::VDeviceInfo > TDeviceInfo; - typedef std::vector< ViconCGStream::VDeviceInfoExtra > TDeviceInfoExtra; - typedef std::vector< ViconCGStream::VChannelInfo > TChannelInfo; - typedef std::vector< ViconCGStream::VChannelInfoExtra > TChannelInfoExtra; - typedef std::vector< ViconCGStream::VForcePlateInfo > TForcePlateInfo; - typedef std::vector< ViconCGStream::VEyeTrackerInfo > TEyeTrackerInfo; - typedef std::map< ViconCGStreamType::UInt32, unsigned int > TIndexMap; - typedef std::pair< ViconCGStreamType::UInt32, ViconCGStreamType::UInt32 > TNestedIDPair; - typedef std::map< TNestedIDPair, unsigned int > TNestedIndexMap; - - TCameraInfo m_CameraInfo; - TCameraCalibrationInfo m_CameraCalibrationInfo; - TCameraCalibrationHealth m_pCameraCalibrationHealth; - TSubjectInfo m_SubjectInfo; - TSubjectTopology m_SubjectTopology; - TSubjectHealth m_SubjectHealth; - TObjectQuality m_ObjectQuality; - TDeviceInfo m_DeviceInfo; - TDeviceInfoExtra m_DeviceInfoExtra; - TChannelInfo m_ChannelInfo; - TChannelInfoExtra m_ChannelInfoExtra; - TForcePlateInfo m_ForcePlateInfo; - TEyeTrackerInfo m_EyeTrackerInfo; - TIndexMap m_CameraMap; - TIndexMap m_CameraCalibrationMap; - TIndexMap m_SubjectMap; - TNestedIndexMap m_SegmentMap; - TIndexMap m_DeviceMap; - TNestedIndexMap m_ChannelMap; - - - ViconCGStream::VCameraInfo & AddCameraInfo(); - ViconCGStream::VCameraCalibrationInfo & AddCameraCalibrationInfo(); - ViconCGStream::VCameraCalibrationHealth & ResetCameraCalibrationHealth(); - ViconCGStream::VSubjectInfo & AddSubjectInfo(); - ViconCGStream::VSubjectTopology & AddSubjectTopology(); - ViconCGStream::VSubjectHealth & AddSubjectHealth(); - ViconCGStream::VObjectQuality & AddObjectQuality(); - ViconCGStream::VDeviceInfo & AddDeviceInfo(); - ViconCGStream::VDeviceInfoExtra & AddDeviceInfoExtra(); - ViconCGStream::VChannelInfo & AddChannelInfo(); - ViconCGStream::VChannelInfoExtra & AddChannelInfoExtra(); - ViconCGStream::VForcePlateInfo & AddForcePlateInfo(); - ViconCGStream::VEyeTrackerInfo & AddEyeTrackerInfo(); - - void BuildMaps(); -}; - -//------------------------------------------------------------------------------------------------- - -class VDynamicObjects -{ -public: - - ViconCGStream::VFrameInfo m_FrameInfo; - ViconCGStream::VHardwareFrameInfo m_HardwareFrameInfo; - ViconCGStream::VTimecode m_Timecode; - ViconCGStream::VLatencyInfo m_LatencyInfo; - ViconCGStream::VFrameRateInfo m_FrameRateInfo; - ViconCGStream::VLabeledRecons m_LabeledRecons; - ViconCGStream::VUnlabeledRecons m_UnlabeledRecons; - ViconCGStream::VLabeledReconRayAssignments m_LabeledRayAssignments; - - std::vector< ViconCGStream::VCentroids > m_Centroids; - std::vector< ViconCGStream::VCentroidTracks > m_CentroidTracks; - std::vector< ViconCGStream::VCentroidWeights > m_CentroidWeights; - std::vector< ViconCGStream::VLocalSegments > m_LocalSegments; - std::vector< ViconCGStream::VGlobalSegments > m_GlobalSegments; - std::vector< ViconCGStream::VGreyscaleBlobs > m_GreyscaleBlobs; - std::vector< ViconCGStream::VEdgePairs > m_EdgePairs; - std::vector< ViconCGStream::VForceFrame > m_ForceFrames; - std::vector< ViconCGStream::VMomentFrame > m_MomentFrames; - std::vector< ViconCGStream::VCentreOfPressureFrame > m_CentreOfPressureFrames; - std::vector< ViconCGStream::VVoltageFrame > m_VoltageFrames; - std::vector< ViconCGStream::VCameraWand2d > m_CameraWand2d; - std::vector< ViconCGStream::VCameraWand3d > m_CameraWand3d; - std::vector< ViconCGStream::VEyeTrackerFrame > m_EyeTrackerFrames; - std::vector< std::shared_ptr< ViconCGStream::VVideoFrame > > m_VideoFrames; - - ViconCGStream::VCentroids & AddCentroids(); - ViconCGStream::VCentroidTracks & AddCentroidTracks(); - ViconCGStream::VCentroidWeights & AddCentroidWeights(); - ViconCGStream::VLocalSegments & AddLocalSegments(); - ViconCGStream::VGlobalSegments & AddGlobalSegments(); - ViconCGStream::VGreyscaleBlobs & AddGreyscaleBlobs(); - ViconCGStream::VEdgePairs & AddEdgePairs(); - ViconCGStream::VForceFrame & AddForceFrame(); - ViconCGStream::VMomentFrame & AddMomentFrame(); - ViconCGStream::VCentreOfPressureFrame & AddCentreOfPressureFrame(); - ViconCGStream::VVoltageFrame & AddVoltageFrame(); - ViconCGStream::VCameraWand2d & AddCameraWand2d(); - ViconCGStream::VCameraWand3d & AddCameraWand3d(); - ViconCGStream::VEyeTrackerFrame & AddEyeTrackerFrame(); - ViconCGStream::VVideoFrame & AddVideoFrame(); -}; - -//------------------------------------------------------------------------------------------------- - -class VViconCGStreamClient -{ -public: - - VViconCGStreamClient( std::weak_ptr< IViconCGStreamClientCallback > i_pCallback ); - ~VViconCGStreamClient(); - - void Connect( const std::string & i_rHost, unsigned short i_Port ); - void Disconnect(); - void ReceiveMulticastData( std::string i_MulticastIPAddress, std::string i_LocalIPAddress, unsigned short i_Port ); - void StopReceivingMulticastData( ); - - void SetStreaming( bool i_bStreaming ); - void SetRequiredObjects( std::set< ViconCGStreamType::Enum > & i_rRequiredObjects ); - void SetApexDeviceFeedback( const std::set< unsigned int > &i_rDeviceList ); - void SetServerToTransmitMulticast( std::string i_MulticastIPAddress, std::string i_ServerIPAddress, unsigned short i_Port ); - void StopMulticastTransmission( ); - void SetFilter( const ViconCGStream::VFilter & i_rFilter ); - - enum EVideoHint { EPassThrough, EDecode }; - void SetVideoHint( EVideoHint i_VideoHint ); - - bool NetworkLatency( double & o_rLatency ) const; - -protected: - - void ClientThread(); - void Run(); - - - bool ReadObjectEnums( VCGStreamReaderWriter & i_rReaderWriter ); - bool WriteObjects( VCGStreamReaderWriter & i_rReaderWriter ); - bool ReadObjects( VCGStreamReaderWriter & i_rReaderWriter ); - - void CopyObjects( const ViconCGStream::VContents & i_rContents, const VStaticObjects & i_rStaticObjects, VStaticObjects & o_rStaticObjects ) const; - void CopyObjects( const ViconCGStream::VContents & i_rContents, const VDynamicObjects & i_rDynamicObjects, VDynamicObjects & o_rDynamicObjects ) const; - - void DecodeVideo( ViconCGStream::VVideoFrame & io_rVideoFrame ); - - void OnConnect() const; - void OnStaticObjects( std::shared_ptr< const VStaticObjects > i_pStaticObjects ) const; - void OnDynamicObjects( std::shared_ptr< const VDynamicObjects > i_pDynamicObjects ) const; - void OnDisconnect() const; - - boost::asio::ip::address_v4 FirstV4AddressFromString( const std::string & i_rAddress ); - - std::weak_ptr< IViconCGStreamClientCallback > m_pCallback; - - boost::asio::io_service m_Service; - std::shared_ptr< boost::asio::ip::tcp::socket > m_pSocket; - std::shared_ptr< boost::asio::ip::udp::socket > m_pMulticastSocket; - - std::shared_ptr< boost::thread > m_pClientThread; - - boost::recursive_mutex m_Mutex; - std::shared_ptr< const VStaticObjects > m_pStaticObjects; - std::shared_ptr< const VDynamicObjects > m_pDynamicObjects; - - ViconCGStream::VObjectEnums m_ServerObjects; - ViconCGStream::VObjectEnums m_RequiredObjects; - ViconCGStream::VFilter m_Filter; - bool m_bEnumsChanged; - bool m_bStreamingChanged; - bool m_bStreaming; - bool m_bHapticChanged; - bool m_bFilterChanged; - - EVideoHint m_VideoHint; - std::vector< unsigned char > m_ScratchVideo; - std::set< unsigned int > m_OnDeviceList; - - std::vector< double > m_GetFrameTimes; -}; diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClient/makefile b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClient/makefile deleted file mode 100644 index 4833f56e9..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClient/makefile +++ /dev/null @@ -1,124 +0,0 @@ -# Makefile for ViconCGStreamClient - -ifndef VERBOSE -.SILENT : -endif -.SUFFIXES : - -ifdef CONFIG -ifneq ($(CONFIG),Debug) -ifneq ($(CONFIG),InternalRelease) -ifneq ($(CONFIG),Release) -Error: unknown configuration. -endif -endif -endif -else -CONFIG=Debug -endif - -INCLUDEPATHS=. ../../../../thirdparty/Boost/boost-1.58.0-dynamic-linux-x64/installed/include .. ../.. -INCLUDEPATHS_Debug=Debug -INCLUDEPATHS_InternalRelease=InternalRelease -INCLUDEPATHS_Release=Release -LIBRARYPATHS=../../../../thirdparty/Boost/boost-1.58.0-dynamic-linux-x64/installed/lib -LIBRARYPATHS_Debug=../../../../lib/Debug ../../../../bin/Debug -LIBRARYPATHS_InternalRelease=../../../../lib/InternalRelease ../../../../bin/InternalRelease -LIBRARYPATHS_Release=../../../../lib/Release ../../../../bin/Release -DEPENDENCIES=ViconCGStream StreamCommon -LIBRARIES= -LIBRARIES_Debug=boost_wserialization-mt-d boost_wave-mt-d boost_unit_test_framework-mt-d boost_timer-mt-d boost_thread-mt-d boost_system-mt-d boost_signals-mt-d boost_serialization-mt-d boost_regex-mt-d boost_random-mt-d boost_python-mt-d boost_program_options-mt-d boost_prg_exec_monitor-mt-d boost_math_tr1l-mt-d boost_math_tr1f-mt-d boost_math_tr1-mt-d boost_math_c99l-mt-d boost_math_c99f-mt-d boost_math_c99-mt-d boost_log_setup-mt-d boost_log-mt-d boost_locale-mt-d boost_iostreams-mt-d boost_graph-mt-d boost_filesystem-mt-d boost_date_time-mt-d boost_coroutine-mt-d boost_context-mt-d boost_container-mt-d boost_chrono-mt-d boost_atomic-mt-d -LIBRARIES_InternalRelease=boost_wserialization-mt boost_wave-mt boost_unit_test_framework-mt boost_timer-mt boost_thread-mt boost_system-mt boost_signals-mt boost_serialization-mt boost_regex-mt boost_random-mt boost_python-mt boost_program_options-mt boost_prg_exec_monitor-mt boost_math_tr1l-mt boost_math_tr1f-mt boost_math_tr1-mt boost_math_c99l-mt boost_math_c99f-mt boost_math_c99-mt boost_log_setup-mt boost_log-mt boost_locale-mt boost_iostreams-mt boost_graph-mt boost_filesystem-mt boost_date_time-mt boost_coroutine-mt boost_context-mt boost_container-mt boost_chrono-mt boost_atomic-mt -LIBRARIES_Release=boost_wserialization-mt boost_wave-mt boost_unit_test_framework-mt boost_timer-mt boost_thread-mt boost_system-mt boost_signals-mt boost_serialization-mt boost_regex-mt boost_random-mt boost_python-mt boost_program_options-mt boost_prg_exec_monitor-mt boost_math_tr1l-mt boost_math_tr1f-mt boost_math_tr1-mt boost_math_c99l-mt boost_math_c99f-mt boost_math_c99-mt boost_log_setup-mt boost_log-mt boost_locale-mt boost_iostreams-mt boost_graph-mt boost_filesystem-mt boost_date_time-mt boost_coroutine-mt boost_context-mt boost_container-mt boost_chrono-mt boost_atomic-mt -DEFINES=TCM_LINUX TCM_UNIX PROJECT_SOURCE_PATH=\"/home/swtest/workspace/DatastreamSDK_Linux_64_Branch_Release/Binary/bin/Release/deployment/Vicon/CrossMarket/DataStream/ViconCGStreamClient\" -DEFINES_Debug=_DEBUG -DEFINES_InternalRelease=VICON_INTERNAL_RELEASE NDEBUG -DEFINES_Release=NDEBUG TCM_OFF_SITE -ENV_CPU=x64 - -SOURCEDIRECTORY=../../../.. -PROJECTPATH=. -BINARYDIRECTORY=../../../.. -INTERMEDIATEDIRECTORY=. -LIBRARYDIRECTORY=../../../../lib -OUTPUTDIRECTORY=../../../../bin - -include $(BINARYDIRECTORY)/gcc.mk - -HIDE_BOOST_SCRIPT=hide_boost_version_script -ifneq ($(HIDE_BOOST),) - HIDE_BOOST_LD_PARAM= -Wl,--version-script=$(HIDE_BOOST_SCRIPT) - HIDE_BOOST_LD_PREREQ=$(HIDE_BOOST_SCRIPT) -endif -all: all_$(CONFIG) - -all_Debug: $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClient.a -all_InternalRelease: $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClient.a -all_Release: $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClient.a - -OBJECTS=$(CONFIG)/ViconCGStreamBayer.o $(CONFIG)/ViconCGStreamClient.o $(CONFIG)/CGStreamReaderWriter.o - -CXXFLAGS=$(CXXFLAGS_$(CONFIG)) $(L_INCLUDEPATHS) $(L_INCLUDEPATHS_$(CONFIG)) $(L_INCLUDES) $(L_INCLUDES_$(CONFIG)) $(L_DEFINES) $(L_DEFINES_$(CONFIG)) -CCFLAGS=$(CCFLAGS_$(CONFIG)) $(L_INCLUDEPATHS) $(L_INCLUDEPATHS_$(CONFIG)) $(L_INCLUDES) $(L_INCLUDES_$(CONFIG)) $(L_DEFINES) $(L_DEFINES_$(CONFIG)) -LDFLAGS=$(LDFLAGS_$(CONFIG)) $(L_LIBRARYPATHS) $(L_LIBRARYPATHS_$(CONFIG)) -CXXFLAGS+=-Wno-unused-local-typedefs -CXXFLAGS+=-Wno-delete-non-virtual-dtor -std=c++14 -#Android toolchain does not include librt but integrates some of its functionality into Android libc. -ifndef ANDROID_TARGET_ARCH -LDFLAGS+=-lrt -endif - - -$(LIBRARYDIRECTORY)/Debug/libViconCGStreamClient.a: makefile $(OBJECTS) - @echo \[1\;35mLinking archive library $@\ - @mkdir -p $(@D) - $(AR) rcs $@ $(OBJECTS) - -$(LIBRARYDIRECTORY)/InternalRelease/libViconCGStreamClient.a: makefile $(OBJECTS) - @echo \[1\;35mLinking archive library $@\ - @mkdir -p $(@D) - $(AR) rcs $@ $(OBJECTS) - -$(LIBRARYDIRECTORY)/Release/libViconCGStreamClient.a: makefile $(OBJECTS) - @echo \[1\;35mLinking archive library $@\ - @mkdir -p $(@D) - $(AR) rcs $@ $(OBJECTS) - -# Source Files -$(CONFIG)/ViconCGStreamBayer.o: makefile $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconCGStreamClient/ViconCGStreamBayer.cpp - @echo \[1\;34mCompiling ViconCGStreamBayer.cpp\ - @mkdir -p $(@D) - find $(CONFIG) -name *.gch -exec cp '{}' . \; - $(CXX) -fPIC -MMD -MP -I$(CONFIG)/ $(CXXFLAGS) -o $@ -c $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconCGStreamClient/ViconCGStreamBayer.cpp - --include $(CONFIG)/ViconCGStreamBayer.d - -$(CONFIG)/ViconCGStreamClient.o: makefile $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconCGStreamClient/ViconCGStreamClient.cpp - @echo \[1\;34mCompiling ViconCGStreamClient.cpp\ - @mkdir -p $(@D) - find $(CONFIG) -name *.gch -exec cp '{}' . \; - $(CXX) -fPIC -MMD -MP -I$(CONFIG)/ $(CXXFLAGS) -o $@ -c $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconCGStreamClient/ViconCGStreamClient.cpp - --include $(CONFIG)/ViconCGStreamClient.d - -$(CONFIG)/CGStreamReaderWriter.o: makefile $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconCGStreamClient/CGStreamReaderWriter.cpp - @echo \[1\;34mCompiling CGStreamReaderWriter.cpp\ - @mkdir -p $(@D) - find $(CONFIG) -name *.gch -exec cp '{}' . \; - $(CXX) -fPIC -MMD -MP -I$(CONFIG)/ $(CXXFLAGS) -o $@ -c $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconCGStreamClient/CGStreamReaderWriter.cpp - --include $(CONFIG)/CGStreamReaderWriter.d - -# Header Files -# Other Files - -clean: - @echo \[1\;31mCleaning $(CONFIG) build\ - find . -path '*/$(CONFIG)/*' \( -name '*.[od]' -o -name '*.gch' \) -exec rm -f {} ';' - rm -f moc_*.cxx - -$(HIDE_BOOST_SCRIPT): makefile - echo -n >$@ - echo "{" >>$@ - echo " local: *N5boost*; *NK5boost*;" >>$@ - echo "};" >>$@ diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClientSDK/CGClient.cpp b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClientSDK/CGClient.cpp deleted file mode 100644 index d47a33f9b..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClientSDK/CGClient.cpp +++ /dev/null @@ -1,505 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#include "CGClient.h" - -#include "ICGFrameState.h" - -#include -#include -#include - -#include - -namespace ViconCGStreamClientSDK -{ - -// Static factory method to create an instance of ICGClient -ICGClient * ICGClient::CreateCGClient() -{ - return new VCGClient(); -} - -/*********************************************************************/ - -// Callback class suitable for having on a weak_ptr -class VCGClientCallback : public IViconCGStreamClientCallback -{ -private: - VCGClient & m_rCGClient; - -public: - VCGClientCallback( VCGClient & i_rCGClient ) - : m_rCGClient( i_rCGClient ) - { - } - - // IViconCGStreamClientCallback - virtual void OnConnect() - { - m_rCGClient.OnConnect(); - } - - virtual void OnStaticObjects( std::shared_ptr< const VStaticObjects > i_pStaticObjects ) - { - m_rCGClient.OnStaticObjects( i_pStaticObjects ); - } - - virtual void OnDynamicObjects( std::shared_ptr< const VDynamicObjects > i_pDynamicObjects ) - { - m_rCGClient.OnDynamicObjects( i_pDynamicObjects ); - } - - virtual void OnDisconnect() - { - m_rCGClient.OnDisconnect(); - } - -}; - -/*********************************************************************/ - -VCGClient::VCGClient() -: m_bConnected( false ) -, m_bMulticastReceiving( false ) -, m_bMulticastController( false ) -, m_MaxBufferSize( 1 ) -, m_bDestroyAfterVideo( false ) -{ - m_pCallback = std::shared_ptr< VCGClientCallback >( new VCGClientCallback( *this ) ); - m_pClient = std::shared_ptr< VViconCGStreamClient >( new VViconCGStreamClient( m_pCallback ) ); -} - -VCGClient::~VCGClient() -{ -} - -void VCGClient::Destroy() -{ - bool bVideoFramesPresent = false; - - { - boost::recursive_mutex::scoped_lock Lock( m_ClientMutex ); - bVideoFramesPresent = !m_VideoFrameReferenceCounted.empty(); - } - - if( bVideoFramesPresent ) - { - m_bDestroyAfterVideo = true; - } - else - { - delete this; - } -} - -bool VCGClient::PollFrames( std::vector< ICGFrameState > & o_rFrames ) -{ - TFrameDeque FrameDeque; - - { - boost::recursive_mutex::scoped_lock Lock( m_ClientMutex ); - std::swap( m_FrameDeque, FrameDeque ); - } - - o_rFrames.resize( FrameDeque.size() ); - if( FrameDeque.empty() ) - { - return false; - } - - TFrameDeque::iterator It = FrameDeque.begin(); - TFrameDeque::iterator End = FrameDeque.end(); - unsigned int Index = 0; - for( ; It != End; ++It, ++Index ) - { - ICGFrameState & rCGFrameState = o_rFrames[ Index ]; - ReadFramePair( *It, rCGFrameState ); - } - return true; -} - -bool VCGClient::PollFrame( ICGFrameState & o_rFrame ) -{ - TFrameDeque FrameDeque; - - TFramePair FramePair; - { - boost::recursive_mutex::scoped_lock Lock( m_ClientMutex ); - if( m_FrameDeque.empty() ) - { - return false; - } - FramePair = m_FrameDeque.front(); - m_FrameDeque.pop_front(); - } - - ReadFramePair( FramePair, o_rFrame ); - return true; -} - -void VCGClient::ReadFramePair( const TFramePair& i_rPair, ICGFrameState& o_rFrameState ) -{ - const std::shared_ptr< const VStaticObjects > & rpStaticState = i_rPair.first; - if ( rpStaticState ) - { - o_rFrameState.m_Stream = rpStaticState->m_StreamInfo; - if( rpStaticState->m_pCameraCalibrationHealth ) - { - o_rFrameState.m_CameraCalibrationHealth = *(rpStaticState->m_pCameraCalibrationHealth); - } - o_rFrameState.m_CameraCalibrations = rpStaticState->m_CameraCalibrationInfo; - o_rFrameState.m_Cameras = rpStaticState->m_CameraInfo; - o_rFrameState.m_Subjects = rpStaticState->m_SubjectInfo; - o_rFrameState.m_SubjectTopologies = rpStaticState->m_SubjectTopology; - o_rFrameState.m_SubjectHealths = rpStaticState->m_SubjectHealth; - o_rFrameState.m_ObjectQualities = rpStaticState->m_ObjectQuality; - o_rFrameState.m_Devices = rpStaticState->m_DeviceInfo; - o_rFrameState.m_DevicesExtra = rpStaticState->m_DeviceInfoExtra; - o_rFrameState.m_Channels = rpStaticState->m_ChannelInfo; - o_rFrameState.m_ChannelUnits = rpStaticState->m_ChannelInfoExtra; - o_rFrameState.m_ForcePlates = rpStaticState->m_ForcePlateInfo; - o_rFrameState.m_EyeTrackers = rpStaticState->m_EyeTrackerInfo; - o_rFrameState.m_ApplicationInfo = rpStaticState->m_ApplicationInfo; - } - - const std::shared_ptr< const VDynamicObjects > & rpDynamicState = i_rPair.second; - if ( rpDynamicState ) - { - o_rFrameState.m_Frame = rpDynamicState->m_FrameInfo; - o_rFrameState.m_HardwareFrame = rpDynamicState->m_HardwareFrameInfo; - o_rFrameState.m_Timecode = rpDynamicState->m_Timecode; - o_rFrameState.m_Latency = rpDynamicState->m_LatencyInfo; - o_rFrameState.m_FrameRateInfo = rpDynamicState->m_FrameRateInfo; - o_rFrameState.m_EdgePairs = rpDynamicState->m_EdgePairs; - o_rFrameState.m_GreyscaleBlobs = rpDynamicState->m_GreyscaleBlobs; - o_rFrameState.m_Centroids = rpDynamicState->m_Centroids; - o_rFrameState.m_CentroidTracks = rpDynamicState->m_CentroidTracks; - o_rFrameState.m_CentroidWeights = rpDynamicState->m_CentroidWeights; - o_rFrameState.m_UnlabeledRecons.m_UnlabeledRecons = rpDynamicState->m_UnlabeledRecons.m_UnlabeledRecons; - o_rFrameState.m_LabeledRecons.m_LabeledRecons = rpDynamicState->m_LabeledRecons.m_LabeledRecons; - o_rFrameState.m_LabeledReconRayAssignments.m_ReconRayAssignments = rpDynamicState->m_LabeledRayAssignments.m_ReconRayAssignments; - o_rFrameState.m_Voltages = rpDynamicState->m_VoltageFrames; - o_rFrameState.m_Forces = rpDynamicState->m_ForceFrames; - o_rFrameState.m_Moments = rpDynamicState->m_MomentFrames; - o_rFrameState.m_CentresOfPressure = rpDynamicState->m_CentreOfPressureFrames; - o_rFrameState.m_GlobalSegments = rpDynamicState->m_GlobalSegments; - o_rFrameState.m_LocalSegments = rpDynamicState->m_LocalSegments; - o_rFrameState.m_CameraWand2d = rpDynamicState->m_CameraWand2d; - o_rFrameState.m_CameraWand3d = rpDynamicState->m_CameraWand3d; - o_rFrameState.m_EyeTracks = rpDynamicState->m_EyeTrackerFrames; - } - - o_rFrameState.m_VideoFrames.clear(); - for( unsigned int Index = 0; rpDynamicState && Index != rpDynamicState->m_VideoFrames.size(); ++Index ) - { - VideoFrameAddRefInitialise( rpDynamicState->m_VideoFrames[ Index ] ); - o_rFrameState.m_VideoFrames.push_back( VVideoFramePtr( this, rpDynamicState->m_VideoFrames[ Index ].get() ) ); - } -} -void VCGClient::VideoFrameAddRefInitialise( std::shared_ptr< const ViconCGStream::VVideoFrame > i_pVideoFrame ) -{ - boost::recursive_mutex::scoped_lock Lock( m_ClientMutex ); - VVideoFrameReferenceCount & rVideoFrameReferenceCount = m_VideoFrameReferenceCounted[ i_pVideoFrame.get() ]; - rVideoFrameReferenceCount.m_pVideoFrame = i_pVideoFrame; - rVideoFrameReferenceCount.m_ReferenceCount = 1; -} - -void VCGClient::VideoFrameAddRef( const ViconCGStream::VVideoFrame * i_pVideoFrame ) -{ - boost::recursive_mutex::scoped_lock Lock( m_ClientMutex ); - VVideoFrameReferenceCount & rVideoFrameReferenceCount = m_VideoFrameReferenceCounted[ i_pVideoFrame ]; - if( !rVideoFrameReferenceCount.m_pVideoFrame ) - { - rVideoFrameReferenceCount.m_pVideoFrame.reset( i_pVideoFrame ); - } - - ++rVideoFrameReferenceCount.m_ReferenceCount; -} - -void VCGClient::VideoFrameRelease( const ViconCGStream::VVideoFrame * i_pVideoFrame ) -{ - boost::recursive_mutex::scoped_lock Lock( m_ClientMutex ); - - TVideoFrameReferenceCounted::iterator It = m_VideoFrameReferenceCounted.find( i_pVideoFrame ); - if( It == m_VideoFrameReferenceCounted.end() ) - { - // This means we've got a logic error - return; - } - - VVideoFrameReferenceCount & rVideoFrameReferenceCount = ( *It ).second; - if( --rVideoFrameReferenceCount.m_ReferenceCount == 0 ) - { - m_VideoFrameReferenceCounted.erase( It ); - } - - if( m_bDestroyAfterVideo && m_VideoFrameReferenceCounted.empty() ) - { - Destroy(); - } -} - -bool VCGClient::NetworkLatency(double & o_rLatency) const -{ - boost::recursive_mutex::scoped_lock Lock(m_ClientMutex); - return m_pClient->NetworkLatency(o_rLatency); -} - -bool VCGClient::WaitFrames( std::vector< ICGFrameState > & o_rFrames, unsigned int i_TimeoutMs ) -{ - boost::recursive_mutex::scoped_lock Lock( m_ClientMutex ); - - boost::xtime WaitDeadline; - - // boost::TIME_UTC has been renamed to boost::TIME_UTC_ in Boost 1.50: - // https://svn.boost.org/trac/boost/ticket/6940 -#if BOOST_VERSION >= 105000 - boost::xtime_get( &WaitDeadline, boost::TIME_UTC_ ); -#else - boost::xtime_get( &WaitDeadline, boost::TIME_UTC ); -#endif - boost::xtime::xtime_sec_t AdditionalSeconds = static_cast< boost::xtime::xtime_sec_t >( static_cast< double >( i_TimeoutMs ) / 1000.0 ); - boost::xtime::xtime_nsec_t AdditionalNanoSeconds = static_cast< boost::xtime::xtime_nsec_t >( ( i_TimeoutMs - ( 1000 * AdditionalSeconds ) ) * 1000000 ); - - WaitDeadline.sec += AdditionalSeconds; - WaitDeadline.nsec += AdditionalNanoSeconds; - - while( m_FrameDeque.empty() ) - { - if( !m_NewFramesCondition.timed_wait( Lock, WaitDeadline ) ) - { - return false; - } - } - - return PollFrames( o_rFrames ); -} - -bool VCGClient::WaitFrame( ICGFrameState& o_rFrame, unsigned int i_TimeoutMs ) -{ - boost::recursive_mutex::scoped_lock Lock( m_ClientMutex ); - - boost::xtime WaitDeadline; - - // boost::TIME_UTC has been renamed to boost::TIME_UTC_ in Boost 1.50: - // https://svn.boost.org/trac/boost/ticket/6940 -#if BOOST_VERSION >= 105000 - boost::xtime_get( &WaitDeadline, boost::TIME_UTC_ ); -#else - boost::xtime_get( &WaitDeadline, boost::TIME_UTC ); -#endif - boost::xtime::xtime_sec_t AdditionalSeconds = static_cast< boost::xtime::xtime_sec_t >( static_cast< double >( i_TimeoutMs ) / 1000.0 ); - boost::xtime::xtime_nsec_t AdditionalNanoSeconds = static_cast< boost::xtime::xtime_nsec_t >( ( i_TimeoutMs - ( 1000 * AdditionalSeconds ) ) * 1000000 ); - - WaitDeadline.sec += AdditionalSeconds; - WaitDeadline.nsec += AdditionalNanoSeconds; - - while( m_FrameDeque.empty() ) - { - if( !m_NewFramesCondition.timed_wait( Lock, WaitDeadline ) ) - { - return false; - } - } - - return PollFrame( o_rFrame ); -} - - - -void VCGClient::Connect( std::string i_IPAddress, unsigned short i_Port ) -{ - boost::recursive_mutex::scoped_lock Lock( m_ClientMutex ); - - m_pClient->Connect( i_IPAddress, i_Port ); -} - -void VCGClient::ReceiveMulticastData( std::string i_MulticastIPAddress, std::string i_LocalIPAddress, unsigned short i_Port ) -{ - boost::recursive_mutex::scoped_lock Lock( m_ClientMutex ); - - m_pClient->ReceiveMulticastData( i_MulticastIPAddress, i_LocalIPAddress, i_Port ); - m_bMulticastReceiving = true; -} - -void VCGClient::StopReceivingMulticastData() -{ - boost::recursive_mutex::scoped_lock Lock( m_ClientMutex ); - - m_pClient->StopReceivingMulticastData( ); - m_bMulticastReceiving = false; -} - -void VCGClient::SetRequestTypes( ViconCGStreamType::Enum i_RequestedType, bool i_bEnable ) -{ - boost::recursive_mutex::scoped_lock Lock( m_ClientMutex ); - - if( i_bEnable ) - { - m_RequestedObjects.m_Enums.insert( i_RequestedType ); - } - else - { - std::set< ViconCGStreamType::Enum >::iterator It = m_RequestedObjects.m_Enums.find( i_RequestedType ); - if( It != m_RequestedObjects.m_Enums.end() ) - { - m_RequestedObjects.m_Enums.erase( It ); - } - } - - m_pClient->SetRequiredObjects( m_RequestedObjects.m_Enums ); -} - -void VCGClient::SetBufferSize( unsigned int i_MaxFrames ) -{ - boost::recursive_mutex::scoped_lock Lock( m_ClientMutex ); - - m_MaxBufferSize = i_MaxFrames; - while( m_FrameDeque.size() > i_MaxFrames ) - { - m_FrameDeque.pop_front(); - } -} - -void VCGClient::SetDecodeVideo( bool i_bDecode ) -{ - boost::recursive_mutex::scoped_lock Lock( m_ClientMutex ); - - m_pClient->SetVideoHint( i_bDecode ? VViconCGStreamClient::EDecode : VViconCGStreamClient::EPassThrough ); -} - -void VCGClient::SetStreamMode( bool i_bStream ) -{ - boost::recursive_mutex::scoped_lock Lock( m_ClientMutex ); - - m_pClient->SetStreaming( i_bStream ); -} - -void VCGClient::SetFilter( const ViconCGStream::VFilter & i_rFilter ) -{ - boost::recursive_mutex::scoped_lock Lock( m_ClientMutex ); - - m_pClient->SetFilter( i_rFilter ); -} - - -void VCGClient::SetServerToTransmitMulticast( std::string i_MulticastIPAddress, std::string i_ServerIPAddress, unsigned short i_Port ) -{ - boost::recursive_mutex::scoped_lock Lock( m_ClientMutex ); - - m_pClient->SetServerToTransmitMulticast( i_MulticastIPAddress, i_ServerIPAddress, i_Port ); - m_bMulticastController = true; -} - -void VCGClient::StopMulticastTransmission() -{ - boost::recursive_mutex::scoped_lock Lock( m_ClientMutex ); - - m_pClient->StopMulticastTransmission(); - m_bMulticastController = false; -} - -bool VCGClient::IsMulticastController() const -{ - boost::recursive_mutex::scoped_lock Lock( m_ClientMutex ); - - return m_bMulticastController; -} - -bool VCGClient::IsConnected() const -{ - boost::recursive_mutex::scoped_lock Lock( m_ClientMutex ); - - return m_bConnected; -} - -bool VCGClient::IsMulticastReceiving() const -{ - boost::recursive_mutex::scoped_lock Lock( m_ClientMutex ); - - return m_bMulticastReceiving; -} - -void VCGClient::OnConnect() -{ - boost::recursive_mutex::scoped_lock Lock( m_ClientMutex ); - - m_bConnected = true; -} - -void VCGClient::OnStaticObjects( std::shared_ptr< const VStaticObjects > i_pStaticObjects ) -{ - boost::recursive_mutex::scoped_lock Lock( m_ClientMutex ); - - m_pLastStaticObjects = i_pStaticObjects; -} - -void VCGClient::OnDynamicObjects( std::shared_ptr< const VDynamicObjects > i_pDynamicObjects ) -{ - boost::recursive_mutex::scoped_lock Lock( m_ClientMutex ); - - m_FrameDeque.push_back( TFramePair( m_pLastStaticObjects, i_pDynamicObjects ) ); - while( m_FrameDeque.size() > m_MaxBufferSize ) - { - m_FrameDeque.pop_front(); - } - - m_NewFramesCondition.notify_all(); -} - -void VCGClient::OnDisconnect() -{ - boost::recursive_mutex::scoped_lock Lock( m_ClientMutex ); - - m_bConnected = false; - m_NewFramesCondition.notify_all(); -} - -bool VCGClient::SetApexDeviceFeedback( unsigned int i_DeviceID, bool i_bOn ) -{ - if( i_bOn ) - { - if( m_HapticDeviceOnList.find( i_DeviceID ) != m_HapticDeviceOnList.end() ) - { - return false; - } - m_HapticDeviceOnList.insert( i_DeviceID ); - } - else - { - std::set< unsigned int >::iterator It = m_HapticDeviceOnList.find( i_DeviceID ); - if( It== m_HapticDeviceOnList.end() ) - { - return false; - } - m_HapticDeviceOnList.erase( It ); - - } - m_pClient->SetApexDeviceFeedback( m_HapticDeviceOnList ); - return true; -} - - -} diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClientSDK/CGClient.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClientSDK/CGClient.h deleted file mode 100644 index 16fac83ad..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClientSDK/CGClient.h +++ /dev/null @@ -1,135 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -#include "ICGClient.h" -#include "ICGFrameState.h" -#include -#include -#include -#include - -namespace ViconCGStreamClientSDK -{ - -class ICGFrameState; -class VCGClientCallback; - -class VCGClient : public ICGClient -{ -public: - - // Constructor - VCGClient(); - virtual ~VCGClient(); - - // ICGClient - virtual void Destroy(); - virtual void Connect( std::string i_IPAddress, unsigned short i_Port ); - virtual void ReceiveMulticastData( std::string i_MulticastIPAddress, std::string i_LocalIPAddress, unsigned short i_Port ); - virtual void StopReceivingMulticastData( ); - - virtual bool IsConnected() const; - virtual bool IsMulticastReceiving() const; - - virtual void SetRequestTypes( ViconCGStreamType::Enum i_RequestedType, bool i_bEnable = true); - virtual void SetBufferSize( unsigned int i_MaxFrames ); - virtual void SetDecodeVideo( bool i_bDecode ); - virtual void SetStreamMode( bool i_bStream ); - virtual void SetServerToTransmitMulticast( std::string i_MulticastIPAddress, std::string i_ServerIPAddress, unsigned short i_Port ); - virtual void StopMulticastTransmission(); - virtual bool IsMulticastController() const; - - virtual bool PollFrames( std::vector< ICGFrameState > & o_rFrames ); - virtual bool PollFrame( ICGFrameState& o_rFrame ); - - virtual bool WaitFrames( std::vector< ICGFrameState > & o_rFrames, unsigned int i_TimeoutMs ); - virtual bool WaitFrame( ICGFrameState& o_rFrame, unsigned int i_TimeoutMs ); - - virtual void VideoFrameAddRef( const ViconCGStream::VVideoFrame * i_pVideoFrame ); - virtual void VideoFrameRelease( const ViconCGStream::VVideoFrame * i_pVideoFrame ); - - virtual bool NetworkLatency(double & o_rLatency) const; - - // maintains a list of devices with haptic feedback on. - // if on add to the list, - // if off delete from the list if existed in the list. - virtual bool SetApexDeviceFeedback( unsigned int i_DeviceID, bool i_bOn ); - - /// Allows filtering of items in a group, e.g. only get centroids for a given camera id, etc. - virtual void SetFilter( const ViconCGStream::VFilter & i_rFilter ); - - // mirrors IViconCGStreamClientCallback - virtual void OnConnect(); - virtual void OnStaticObjects( std::shared_ptr< const VStaticObjects > i_pStaticObjects ); - virtual void OnDynamicObjects( std::shared_ptr< const VDynamicObjects > i_pDynamicObjects ); - virtual void OnDisconnect(); - -protected: - - // Buffer - typedef std::pair< std::shared_ptr< const VStaticObjects >, std::shared_ptr< const VDynamicObjects > > TFramePair; - typedef std::deque< TFramePair > TFrameDeque; - - void ReadFramePair( const TFramePair& i_rPair, ICGFrameState& o_rFrameState ); - void VideoFrameAddRefInitialise( std::shared_ptr< const ViconCGStream::VVideoFrame > i_pVideoFrame ); - - // The C++ client which does all of the work for us - std::shared_ptr< VViconCGStreamClient > m_pClient; - std::shared_ptr< VCGClientCallback > m_pCallback; - bool m_bConnected; - bool m_bMulticastReceiving; - bool m_bMulticastController; - std::set< unsigned int > m_HapticDeviceOnList; - - // Configuration - mutable boost::recursive_mutex m_ClientMutex; - ViconCGStream::VObjectEnums m_RequestedObjects; - - std::shared_ptr< const VStaticObjects > m_pLastStaticObjects; - TFrameDeque m_FrameDeque; - unsigned int m_MaxBufferSize; - - boost::condition m_NewFramesCondition; - - - class VVideoFrameReferenceCount - { - public: - VVideoFrameReferenceCount() - : m_ReferenceCount( 0 ) - { - } - int m_ReferenceCount; - std::shared_ptr< const ViconCGStream::VVideoFrame > m_pVideoFrame; - }; - - typedef std::map< const ViconCGStream::VVideoFrame *, VVideoFrameReferenceCount > TVideoFrameReferenceCounted; - TVideoFrameReferenceCounted m_VideoFrameReferenceCounted; - - bool m_bDestroyAfterVideo; -}; - -} // End of namespace ViconCGStreamClientSDK diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClientSDK/ICGClient.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClientSDK/ICGClient.h deleted file mode 100644 index c62c41aea..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClientSDK/ICGClient.h +++ /dev/null @@ -1,126 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -#include -#include -#include - -namespace ViconCGStream -{ - class VVideoFrame; - class VFilter; -}; - -namespace ViconCGStreamClientSDK -{ - -class ICGFrameState; - -class ICGClient -{ -public: - - /// Create an instance of the client - static ICGClient * CreateCGClient(); - - /// Destroy this instance of the client - virtual void Destroy() = 0; - - /// Set the ipAddress to connect to and begin connection attempts - virtual void Connect( std::string i_IPAddress, unsigned short i_Port ) = 0; - - /// Configure this CGClient to be a multicast receiver. - /// Users should call either ReceiveMulticastData or Connect, not both. - /// i_MulticastIPAddress is the address that the server will send data to (and may be the broadcast address). - /// i_LocalIPAddress is the local IP address, used to specify which NIC should listen. - virtual void ReceiveMulticastData( std::string i_MulticastIPAddress, std::string i_LocalIPAddress, unsigned short i_Port ) = 0; - - /// Stop this CGClient from receiving multicast data - /// After calling this function users should call either ReceiveMulticastData or Connect. - virtual void StopReceivingMulticastData( ) = 0; - - /// Return whether we are connected to a server - virtual bool IsConnected() const = 0; - - /// Return whether we are receiving multicast data (note that broadcast counts as multicast) - virtual bool IsMulticastReceiving() const = 0; - - /// Set the types ( ViconCGStreamType::Enum ) of data to be received from the server - virtual void SetRequestTypes( ViconCGStreamType::Enum i_RequestedType, bool i_bEnable = true) = 0; - - /// Set the maximum number of frames you want cached - virtual void SetBufferSize( unsigned int i_MaxFrames ) = 0; - - /// Request that video data be transcoded into BGR888 - virtual void SetDecodeVideo( bool i_bDecode ) = 0; - - /// Request that data is constantly streamed from the server, rather than sent on request. - virtual void SetStreamMode( bool i_bStream ) = 0; - - /// Request for turning apex device feedback on or off - virtual bool SetApexDeviceFeedback( unsigned int i_DeviceID, bool i_bOn ) = 0; - - /// Allows filtering of items in a group, e.g. only get centroids for a given camera id, etc. - virtual void SetFilter( const ViconCGStream::VFilter & i_rFilter ) = 0; - - /// Set the server to transmit via multicast UDP - /// Should be called AFTER calling Connect (and another instance of the ICGClient should have called ReceiveMulticastData) - /// i_MulticastIPAddress is the address that the server will send data to (and may be the broadcast address). - /// i_ServerIPAddress is the IP address that the server should transmit from, used to specify which NIC should be used by the server. - /// This does not affect the connected stream from the server to this client. - virtual void SetServerToTransmitMulticast( std::string i_MulticastIPAddress, std::string i_ServerIPAddress, unsigned short i_Port ) = 0; - - /// Signal the server to stop transmitting multicast data. - /// This does not affect the connected stream from the server to this client. - /// It is not necessary to call StopMulticastTransmission before calling Destroy. - virtual void StopMulticastTransmission() = 0; - - /// Determine whether this client is acting as a Multicast controller - virtual bool IsMulticastController() const = 0; - - /// Poll for frames. Returns immediately - virtual bool PollFrame( ICGFrameState& o_rFrame ) = 0; - virtual bool PollFrames( std::vector< ICGFrameState > & o_rFrames ) = 0; - - /// Wait for frames. Returns when a frame arrives or i_TimeoutMs expires - virtual bool WaitFrame( ICGFrameState& o_rFrame, unsigned int i_TimeoutMs ) = 0; - virtual bool WaitFrames( std::vector< ICGFrameState > & o_rFrames, unsigned int i_TimeoutMs ) = 0; - - /// Increment video frame reference count - virtual void VideoFrameAddRef( const ViconCGStream::VVideoFrame * i_pVideoFrame ) = 0; - - /// Decrement video frame reference count - virtual void VideoFrameRelease( const ViconCGStream::VVideoFrame * i_pVideoFrame ) = 0; - - /// Returns our current estimate of network latency - virtual bool NetworkLatency(double & o_rLatency) const = 0; - -protected: -/// Private desctructor. To delete, call the Destroy() method - virtual ~ICGClient() {} -}; - -} // End of namespace ViconCGStreamClientSDK diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClientSDK/ICGFrameState.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClientSDK/ICGFrameState.h deleted file mode 100644 index 70abbda12..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClientSDK/ICGFrameState.h +++ /dev/null @@ -1,196 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "ICGClient.h" - -namespace ViconCGStreamClientSDK -{ - -class VVideoFramePtr -{ -public: - - VVideoFramePtr() - : m_pClient( 0 ) - , m_pVideoFrame( 0 ) - { - } - - VVideoFramePtr( ICGClient * i_pClient, ViconCGStream::VVideoFrame * i_pVideoFrame ) - : m_pClient( i_pClient ) - , m_pVideoFrame( i_pVideoFrame ) - { - } - - VVideoFramePtr( const VVideoFramePtr & i_rVideoFramePtr ) - : m_pClient( i_rVideoFramePtr.m_pClient ) - , m_pVideoFrame( i_rVideoFramePtr.m_pVideoFrame ) - { - if ( m_pClient && m_pVideoFrame ) - { - m_pClient->VideoFrameAddRef( m_pVideoFrame ); - } - } - - ~VVideoFramePtr() - { - Clear(); - } - - const ViconCGStream::VVideoFrame & operator * () const - { - return *m_pVideoFrame; - } - - ViconCGStream::VVideoFrame & operator * () - { - return *m_pVideoFrame; - } - - explicit operator bool() - { - return m_pClient && m_pVideoFrame; - } - - void Clear() - { - if( m_pClient && m_pVideoFrame ) - { - m_pClient->VideoFrameRelease( m_pVideoFrame ); - m_pClient = 0; - m_pVideoFrame = 0; - } - } - -private: - - ICGClient * m_pClient; - ViconCGStream::VVideoFrame * m_pVideoFrame; -}; - - -class ICGFrameState -{ -public: - // Frame - ViconCGStream::VStreamInfo m_Stream; - ViconCGStream::VFrameInfo m_Frame; - ViconCGStream::VHardwareFrameInfo m_HardwareFrame; - ViconCGStream::VTimecode m_Timecode; - ViconCGStream::VLatencyInfo m_Latency; - boost::optional m_ApplicationInfo; - ViconCGStream::VFrameRateInfo m_FrameRateInfo; - - // Cameras - ViconCGStream::VCameraCalibrationHealth m_CameraCalibrationHealth; - std::vector< ViconCGStream::VCameraCalibrationInfo > m_CameraCalibrations; - std::vector< ViconCGStream::VCameraInfo > m_Cameras; - std::vector< ViconCGStream::VEdgePairs > m_EdgePairs; - std::vector< ViconCGStream::VGreyscaleBlobs > m_GreyscaleBlobs; - std::vector< ViconCGStream::VCentroids > m_Centroids; - std::vector< ViconCGStream::VCentroidTracks > m_CentroidTracks; - std::vector< ViconCGStream::VCentroidWeights > m_CentroidWeights; - std::vector< VVideoFramePtr > m_VideoFrames; - std::vector< ViconCGStream::VCameraWand2d > m_CameraWand2d; - std::vector< ViconCGStream::VCameraWand3d > m_CameraWand3d; - - // Reconstructions - ViconCGStream::VUnlabeledRecons m_UnlabeledRecons; - ViconCGStream::VLabeledRecons m_LabeledRecons; - ViconCGStream::VLabeledReconRayAssignments m_LabeledReconRayAssignments; - - // Devices - std::vector< ViconCGStream::VDeviceInfo > m_Devices; - std::vector< ViconCGStream::VDeviceInfoExtra > m_DevicesExtra; - std::vector< ViconCGStream::VChannelInfo > m_Channels; - std::vector< ViconCGStream::VChannelInfoExtra > m_ChannelUnits; - std::vector< ViconCGStream::VVoltageFrame > m_Voltages; - - // Force Plates - std::vector< ViconCGStream::VForcePlateInfo > m_ForcePlates; - std::vector< ViconCGStream::VForceFrame > m_Forces; - std::vector< ViconCGStream::VMomentFrame > m_Moments; - std::vector< ViconCGStream::VCentreOfPressureFrame > m_CentresOfPressure; - - // Eye Trackers - std::vector< ViconCGStream::VEyeTrackerInfo > m_EyeTrackers; - std::vector< ViconCGStream::VEyeTrackerFrame > m_EyeTracks; - - // Subjects - std::vector< ViconCGStream::VSubjectInfo > m_Subjects; - std::vector< ViconCGStream::VSubjectTopology > m_SubjectTopologies; - std::vector< ViconCGStream::VSubjectHealth > m_SubjectHealths; - std::vector< ViconCGStream::VObjectQuality > m_ObjectQualities; - std::vector< ViconCGStream::VGlobalSegments > m_GlobalSegments; - std::vector< ViconCGStream::VLocalSegments > m_LocalSegments; -}; - -} // End of namespace ViconCGStreamClientSDK diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClientSDK/makefile b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClientSDK/makefile deleted file mode 100644 index da6b9cb26..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconCGStreamClientSDK/makefile +++ /dev/null @@ -1,108 +0,0 @@ -# Makefile for ViconCGStreamClientSDK - -ifndef VERBOSE -.SILENT : -endif -.SUFFIXES : - -ifdef CONFIG -ifneq ($(CONFIG),Debug) -ifneq ($(CONFIG),InternalRelease) -ifneq ($(CONFIG),Release) -Error: unknown configuration. -endif -endif -endif -else -CONFIG=Debug -endif - -INCLUDEPATHS=../../../../thirdparty/Boost/boost-1.58.0-dynamic-linux-x64/installed/include . .. ../.. -INCLUDEPATHS_Debug=Debug -INCLUDEPATHS_InternalRelease=InternalRelease -INCLUDEPATHS_Release=Release -LIBRARYPATHS=../../../../thirdparty/Boost/boost-1.58.0-dynamic-linux-x64/installed/lib -LIBRARYPATHS_Debug=../../../../lib/Debug ../../../../bin/Debug -LIBRARYPATHS_InternalRelease=../../../../lib/InternalRelease ../../../../bin/InternalRelease -LIBRARYPATHS_Release=../../../../lib/Release ../../../../bin/Release -DEPENDENCIES=ViconCGStreamClient ViconCGStream StreamCommon -LIBRARIES= -LIBRARIES_Debug=boost_wserialization-mt-d boost_wave-mt-d boost_unit_test_framework-mt-d boost_timer-mt-d boost_thread-mt-d boost_system-mt-d boost_signals-mt-d boost_serialization-mt-d boost_regex-mt-d boost_random-mt-d boost_python-mt-d boost_program_options-mt-d boost_prg_exec_monitor-mt-d boost_math_tr1l-mt-d boost_math_tr1f-mt-d boost_math_tr1-mt-d boost_math_c99l-mt-d boost_math_c99f-mt-d boost_math_c99-mt-d boost_log_setup-mt-d boost_log-mt-d boost_locale-mt-d boost_iostreams-mt-d boost_graph-mt-d boost_filesystem-mt-d boost_date_time-mt-d boost_coroutine-mt-d boost_context-mt-d boost_container-mt-d boost_chrono-mt-d boost_atomic-mt-d -LIBRARIES_InternalRelease=boost_wserialization-mt boost_wave-mt boost_unit_test_framework-mt boost_timer-mt boost_thread-mt boost_system-mt boost_signals-mt boost_serialization-mt boost_regex-mt boost_random-mt boost_python-mt boost_program_options-mt boost_prg_exec_monitor-mt boost_math_tr1l-mt boost_math_tr1f-mt boost_math_tr1-mt boost_math_c99l-mt boost_math_c99f-mt boost_math_c99-mt boost_log_setup-mt boost_log-mt boost_locale-mt boost_iostreams-mt boost_graph-mt boost_filesystem-mt boost_date_time-mt boost_coroutine-mt boost_context-mt boost_container-mt boost_chrono-mt boost_atomic-mt -LIBRARIES_Release=boost_wserialization-mt boost_wave-mt boost_unit_test_framework-mt boost_timer-mt boost_thread-mt boost_system-mt boost_signals-mt boost_serialization-mt boost_regex-mt boost_random-mt boost_python-mt boost_program_options-mt boost_prg_exec_monitor-mt boost_math_tr1l-mt boost_math_tr1f-mt boost_math_tr1-mt boost_math_c99l-mt boost_math_c99f-mt boost_math_c99-mt boost_log_setup-mt boost_log-mt boost_locale-mt boost_iostreams-mt boost_graph-mt boost_filesystem-mt boost_date_time-mt boost_coroutine-mt boost_context-mt boost_container-mt boost_chrono-mt boost_atomic-mt -DEFINES=TCM_LINUX TCM_UNIX PROJECT_SOURCE_PATH=\"/home/swtest/workspace/DatastreamSDK_Linux_64_Branch_Release/Binary/bin/Release/deployment/Vicon/CrossMarket/DataStream/ViconCGStreamClientSDK\" -DEFINES_Debug=_DEBUG -DEFINES_InternalRelease=VICON_INTERNAL_RELEASE NDEBUG -DEFINES_Release=NDEBUG TCM_OFF_SITE -ENV_CPU=x64 - -SOURCEDIRECTORY=../../../.. -PROJECTPATH=. -BINARYDIRECTORY=../../../.. -INTERMEDIATEDIRECTORY=. -LIBRARYDIRECTORY=../../../../lib -OUTPUTDIRECTORY=../../../../bin - -include $(BINARYDIRECTORY)/gcc.mk - -HIDE_BOOST_SCRIPT=hide_boost_version_script -ifneq ($(HIDE_BOOST),) - HIDE_BOOST_LD_PARAM= -Wl,--version-script=$(HIDE_BOOST_SCRIPT) - HIDE_BOOST_LD_PREREQ=$(HIDE_BOOST_SCRIPT) -endif -all: all_$(CONFIG) - -all_Debug: $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClientSDK.a -all_InternalRelease: $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClientSDK.a -all_Release: $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClientSDK.a - -OBJECTS=$(CONFIG)/CGClient.o - -CXXFLAGS=$(CXXFLAGS_$(CONFIG)) $(L_INCLUDEPATHS) $(L_INCLUDEPATHS_$(CONFIG)) $(L_INCLUDES) $(L_INCLUDES_$(CONFIG)) $(L_DEFINES) $(L_DEFINES_$(CONFIG)) -CCFLAGS=$(CCFLAGS_$(CONFIG)) $(L_INCLUDEPATHS) $(L_INCLUDEPATHS_$(CONFIG)) $(L_INCLUDES) $(L_INCLUDES_$(CONFIG)) $(L_DEFINES) $(L_DEFINES_$(CONFIG)) -LDFLAGS=$(LDFLAGS_$(CONFIG)) $(L_LIBRARYPATHS) $(L_LIBRARYPATHS_$(CONFIG)) -CXXFLAGS+=-Wno-unused-local-typedefs -CXXFLAGS+=-Wno-delete-non-virtual-dtor -std=c++14 -#Android toolchain does not include librt but integrates some of its functionality into Android libc. -ifndef ANDROID_TARGET_ARCH -LDFLAGS+=-lrt -endif - - -$(LIBRARYDIRECTORY)/Debug/libViconCGStreamClientSDK.a: makefile $(OBJECTS) - @echo \[1\;35mLinking archive library $@\ - @mkdir -p $(@D) - $(AR) rcs $@ $(OBJECTS) - -$(LIBRARYDIRECTORY)/InternalRelease/libViconCGStreamClientSDK.a: makefile $(OBJECTS) - @echo \[1\;35mLinking archive library $@\ - @mkdir -p $(@D) - $(AR) rcs $@ $(OBJECTS) - -$(LIBRARYDIRECTORY)/Release/libViconCGStreamClientSDK.a: makefile $(OBJECTS) - @echo \[1\;35mLinking archive library $@\ - @mkdir -p $(@D) - $(AR) rcs $@ $(OBJECTS) - -# Source Files -$(CONFIG)/CGClient.o: makefile $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconCGStreamClientSDK/CGClient.cpp - @echo \[1\;34mCompiling CGClient.cpp\ - @mkdir -p $(@D) - find $(CONFIG) -name *.gch -exec cp '{}' . \; - $(CXX) -fPIC -MMD -MP -I$(CONFIG)/ $(CXXFLAGS) -o $@ -c $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconCGStreamClientSDK/CGClient.cpp - --include $(CONFIG)/CGClient.d - -# Header Files -# Other Files - -clean: - @echo \[1\;31mCleaning $(CONFIG) build\ - find . -path '*/$(CONFIG)/*' \( -name '*.[od]' -o -name '*.gch' \) -exec rm -f {} ';' - rm -f moc_*.cxx - -$(HIDE_BOOST_SCRIPT): makefile - echo -n >$@ - echo "{" >>$@ - echo " local: *N5boost*; *NK5boost*;" >>$@ - echo "};" >>$@ diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/AxisMapping.cpp b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/AxisMapping.cpp deleted file mode 100644 index d7b4cc7af..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/AxisMapping.cpp +++ /dev/null @@ -1,184 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#include "AxisMapping.h" - -#include "ClientUtils.h" - -#include - -using namespace ClientUtils; - -namespace ViconDataStreamSDK -{ -namespace Core -{ - -VAxisMapping::VAxisMapping() -{ -} - -VAxisMapping::~VAxisMapping() -{ -} - -std::shared_ptr< VAxisMapping > VAxisMapping::Create( AxisMappingResult::Enum & o_rError, - Direction::Enum i_XAxis, - Direction::Enum i_YAxis, - Direction::Enum i_ZAxis ) -{ - std::shared_ptr< VAxisMapping > pAxisMapping( new VAxisMapping() ); - - o_rError = pAxisMapping->SetAxisMapping( i_XAxis, i_YAxis, i_ZAxis ); - if( o_rError != AxisMappingResult::Success ) - { - pAxisMapping.reset(); - } - - return pAxisMapping; -} - -AxisMappingResult::Enum VAxisMapping::SetAxisMapping( const Direction::Enum i_XAxis, - const Direction::Enum i_YAxis, - const Direction::Enum i_ZAxis ) -{ - // Check for co-linear input - { - std::vector< unsigned int > Axes( 3, 0 ); - ++Axes[ ComponentIndex( i_XAxis ) ]; - ++Axes[ ComponentIndex( i_YAxis ) ]; - ++Axes[ ComponentIndex( i_ZAxis ) ]; - - std::vector< unsigned int > ExpectedAxes( 3, 1 ); - - if( Axes != ExpectedAxes ) - { - return AxisMappingResult::CoLinearAxes; - } - } - - // Check that they do not specify a left handed co-ordinate system - { - boost::array< double, 3 > XAxis; - std::fill( XAxis.begin(), XAxis.end(), 0.0 ); - XAxis[ ComponentIndex( i_XAxis ) ] = ComponentSign( i_XAxis ); - - boost::array< double, 3 > YAxis; - std::fill( YAxis.begin(), YAxis.end(), 0.0 ); - YAxis[ ComponentIndex( i_YAxis ) ] = ComponentSign( i_YAxis ); - - boost::array< double, 3 > ZAxis; - std::fill( ZAxis.begin(), ZAxis.end(), 0.0 ); - ZAxis[ ComponentIndex( i_ZAxis ) ] = ComponentSign( i_ZAxis ); - - // In a right handed coordinate system, ZAxis == XAxis x YAxis - if( ZAxis != ( CrossProduct( XAxis, YAxis ) ) ) - { - return AxisMappingResult::LeftHandedAxes; - } - } - - // Make a transform matrix - std::fill( m_Transform, m_Transform + 9, 0.0 ); - m_Transform[ 0 + ComponentIndex( i_XAxis ) ] = ComponentSign( i_XAxis ); - m_Transform[ 3 + ComponentIndex( i_YAxis ) ] = ComponentSign( i_YAxis ); - m_Transform[ 6 + ComponentIndex( i_ZAxis ) ] = ComponentSign( i_ZAxis ); - - // Store away the directions in case anyone asks us later - m_XAxis = i_XAxis; - m_YAxis = i_YAxis; - m_ZAxis = i_ZAxis; - - return AxisMappingResult::Success; -} - -void VAxisMapping::GetAxisMapping( Direction::Enum & o_rXAxis, Direction::Enum & o_rYAxis, Direction::Enum & o_rZAxis ) const -{ - o_rXAxis = m_XAxis; - o_rYAxis = m_YAxis; - o_rZAxis = m_ZAxis; -} - -void VAxisMapping::CopyAndTransformT( const double i_Translation[ 3 ], double ( & io_Translation )[ 3 ] ) const -{ - boost::array< double, 9 > Tm; - std::copy( m_Transform, m_Transform + 9, Tm.begin() ); - boost::array< double, 3 > T; - std::copy( i_Translation, i_Translation + 3, T.begin() ); - boost::array< double, 3 > Result = Tm * T; - std::copy( Result.begin(), Result.end(), &io_Translation[0] ); -} - -void VAxisMapping::CopyAndTransformR( const double i_Rotation[ 9 ], double ( & io_Rotation )[ 9 ] ) const -{ - boost::array< double, 9 > R; - std::copy( i_Rotation, i_Rotation+9, R.begin() ); - boost::array< double, 9 > T; - std::copy( m_Transform, m_Transform+9, T.begin() ); - boost::array< double, 9 > Result = T * R * Transpose( T ); - std::copy(Result.begin(), Result.end(), &io_Rotation[0] ); -} - -// From server to flow -// Axis(server).transpose * WorldPose(server) = Axis(server).transpose * Rotation(server) * LocalRotation(server) + Axis( server).transpose * Translation(server) -// Axis(server).transpose * WorldPose(server) = Axis(server).transpose * Rotation(server) * Axis(server) * Axis(server).transpose * LocalRotation(server) + Axis( server).transpose * Translation(server) -// WorldPose(server) = Axis(Server).transpose * Rotation(server) * Axis( server ) * LocaRotation(Flow) + Translation(flow) - -// from flow to client -// Axis( client) * Axis(server).transpose * WorldPose(server) = Axis( Client) * Axis(server).transpose * Rotation(server) * Axis(server) * Axis(server).transpose * LocalRotation(server) + Axis( client ) * Axis( server).transpose * Translation(server) -// Axis( client) * Axis(server).transpose * WorldPose(server) = Axis( Client) * Axis(server).transpose * Rotation(server) * Axis(server) * Axis( client).transpose * Axis( client ) * Axis(server).transpose * LocalRotation(server) + Axis( client ) * Axis( server).transpose * Translation(server) -// WorldPose( client) = Axis( Client) * Axis(server).transpose * Rotation(server) * Axis(server) * Axis( client).transpose * LocalRotation(server) + Translation(client) - -void VAxisMapping::CopyAndTransformT( const double i_Translation[3], const double i_Orientation[9], double( &io_Translation )[3] ) const -{ - boost::array< double, 9 > Q; - std::copy( i_Orientation, i_Orientation + 9, Q.begin() ); - boost::array< double, 9 > Tm; - std::copy( m_Transform, m_Transform + 9, Tm.begin() ); - boost::array< double, 3 > T; - std::copy( i_Translation, i_Translation + 3, T.begin() ); - boost::array< double, 3 > Result = Tm * Transpose( Q ) * T; - std::copy( Result.begin(), Result.end(), &io_Translation[0] ); -} - - -void VAxisMapping::CopyAndTransformR( const double i_Rotation[9], const double i_Orientation[9], double( &io_Rotation )[9] ) const -{ - boost::array< double, 9 > Q; - std::copy( i_Orientation, i_Orientation + 9, Q.begin() ); - boost::array< double, 9 > R; - std::copy( i_Rotation, i_Rotation + 9, R.begin() ); - boost::array< double, 9 > T; - std::copy( m_Transform, m_Transform + 9, T.begin() ); - boost::array< double, 9 > Result = T * Transpose( Q ) * R * Q * Transpose( T ) ; - std::copy( Result.begin(), Result.end(), &io_Rotation[0] ); -} - -void VAxisMapping::GetTransformationMatrix( double( &o_Rotation )[9] ) -{ - std::copy( m_Transform, m_Transform + 9, o_Rotation ); -} - -} // End of namespace Core -} // End of namespace ViconDataStreamSDK diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/AxisMapping.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/AxisMapping.h deleted file mode 100644 index db0949665..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/AxisMapping.h +++ /dev/null @@ -1,76 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -#include "Constants.h" -#include - -namespace ViconDataStreamSDK -{ -namespace Core -{ - -class VAxisMapping -{ -public: - virtual ~VAxisMapping(); - - // Create a mapper and set the transformation matrix - // Returns a null shared_ptr if there is an error in the axis system - static std::shared_ptr< VAxisMapping > Create( AxisMappingResult::Enum & o_rError, - Direction::Enum i_XAxis, - Direction::Enum i_YAxis, - Direction::Enum i_ZAxis ); - - // Set the transform properties - void GetAxisMapping( Direction::Enum & o_rXAxis, Direction::Enum & o_rYAxis, Direction::Enum & o_rZAxis ) const; - - // Transform world points and rotations - void CopyAndTransformT( const double i_Translation[ 3 ], double ( & io_Translation )[ 3 ] ) const; - void CopyAndTransformR( const double i_Rotation[ 9 ], double ( & io_Rotation )[ 9 ] ) const; - - void CopyAndTransformT( const double i_Translation[3], const double i_Orientation[9], double( &io_Translation )[3] ) const; - void CopyAndTransformR( const double i_Rotation[9], const double i_Orientation[9], double( &io_Rotation )[9] ) const; - - // Get the underlying transformation matrix - void GetTransformationMatrix( double( &o_Rotation )[9] ); - -private: - VAxisMapping(); - - // Set the transform matrix - AxisMappingResult::Enum SetAxisMapping( const Direction::Enum i_XAxis, - const Direction::Enum i_YAxis, - const Direction::Enum i_ZAxis ); - - // Raw axis mappings and equivalent transform matrix - Direction::Enum m_XAxis; - Direction::Enum m_YAxis; - Direction::Enum m_ZAxis; - double m_Transform[ 9 ]; -}; - -} // End of namespace Core -} // End of namespace ViconDataStreamSDK diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/ClientUtils.cpp b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/ClientUtils.cpp deleted file mode 100644 index 39d4a1c3d..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/ClientUtils.cpp +++ /dev/null @@ -1,400 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// - - -#include "ClientUtils.h" - - -#include -#include -#include - -#include - -namespace ClientUtils -{ - // These functions clear a value - void Clear( bool & o_rValue ) - { - o_rValue = false; - } - - void Clear( unsigned int & o_rValue ) - { - o_rValue = 0; - } - - void Clear( double & o_rValue ) - { - o_rValue = 0.0; - } - - void Clear( std::string & o_rValue ) - { - o_rValue.clear(); - } - - void Clear( ViconDataStreamSDK::Core::TimecodeStandard::Enum & o_rValue ) - { - o_rValue = ViconDataStreamSDK::Core::TimecodeStandard::None; - } - - void Clear( ViconDataStreamSDK::Core::DeviceType::Enum & o_rValue ) - { - o_rValue = ViconDataStreamSDK::Core::DeviceType::Unknown; - } - - void Clear( ViconDataStreamSDK::Core::Unit::Enum & o_rValue ) - { - o_rValue = ViconDataStreamSDK::Core::Unit::Unknown; - } - - // Determine whether an axis is positive or negative - double ComponentSign( ViconDataStreamSDK::Core::Direction::Enum i_Direction ) - { - switch( i_Direction ) - { - case ViconDataStreamSDK::Core::Direction::Forward : - case ViconDataStreamSDK::Core::Direction::Left : - case ViconDataStreamSDK::Core::Direction::Up : return 1.0; - case ViconDataStreamSDK::Core::Direction::Backward: - case ViconDataStreamSDK::Core::Direction::Right : - case ViconDataStreamSDK::Core::Direction::Down : return -1.0; - default : assert( !"Unknown enum value" ); - return 0.0; - } - } - - // Determine the index of an enum - unsigned int ComponentIndex( ViconDataStreamSDK::Core::Direction::Enum i_Direction ) - { - switch( i_Direction ) - { - case ViconDataStreamSDK::Core::Direction::Forward : - case ViconDataStreamSDK::Core::Direction::Backward: return 0; - case ViconDataStreamSDK::Core::Direction::Left : - case ViconDataStreamSDK::Core::Direction::Right : return 1; - case ViconDataStreamSDK::Core::Direction::Up : - case ViconDataStreamSDK::Core::Direction::Down : return 2; - default : assert( !"Unknown enum value" ); - return 0; - } - } - - std::string ComponentName( ViconDataStreamSDK::Core::Direction::Enum i_Direction ) - { - switch( i_Direction ) - { - case ViconDataStreamSDK::Core::Direction::Forward: return "Forward"; - case ViconDataStreamSDK::Core::Direction::Backward: return "Backward"; - case ViconDataStreamSDK::Core::Direction::Left: return "Left"; - case ViconDataStreamSDK::Core::Direction::Right: return "Right"; - case ViconDataStreamSDK::Core::Direction::Up: return "Up"; - case ViconDataStreamSDK::Core::Direction::Down: return "Down"; - default: assert( !"Unknown enum value" ); - return ""; - } - - } - - std::string AdaptCameraName( const std::string & i_rCameraName, - const std::string & i_rDisplayType, - const unsigned int i_CameraID ) - { - // Append Camera info to ensure uniqueness, since this name is used as keys in the DSSDK Camera APIs - return str( boost::format( "%s (%d)" ) %( i_rCameraName.empty()? i_rDisplayType : i_rCameraName ) %i_CameraID ); - } - - std::string AdaptDeviceName( const std::string & i_rDeviceName, - const unsigned int i_DeviceID ) - { - if( !i_rDeviceName.empty() ) - { - return i_rDeviceName; - } - else - { - return str( boost::format( "Unnamed Device %d" ) % i_DeviceID ); - } - } - - std::string AdaptDeviceOutputName( const std::string & i_rDeviceOutputName, - const unsigned int i_DeviceOutputIndex ) - { - if( !i_rDeviceOutputName.empty() ) - { - return i_rDeviceOutputName; - } - else - { - return str( boost::format( "Unnamed Device Output %d" ) % ( i_DeviceOutputIndex + 1 ) ); - } - } - - bool IsValidMulticastIP( const std::string & i_MulticastIPAddress ) - { - boost::asio::io_service Service; - boost::system::error_code Error; - boost::asio::ip::address_v4 Address = boost::asio::ip::address_v4::from_string( i_MulticastIPAddress, Error ); - if( Error ) - { - boost::asio::ip::tcp::resolver Resolver( Service ); - boost::asio::ip::tcp::resolver::query Query( i_MulticastIPAddress, "" ); - - boost::asio::ip::tcp::resolver::iterator It = Resolver.resolve( Query, Error ); - boost::asio::ip::tcp::resolver::iterator End; - - if( ! Error ) - { - for( ; It != End; ++It ) - { - Error = boost::system::error_code(); - boost::asio::ip::tcp::endpoint EndPoint( *It ); - - // Currently we only handle IPv4 - if( EndPoint.address().is_v4() ) - { - Address = EndPoint.address().to_v4(); - break; - } - } - } - else - { - Address = boost::asio::ip::address_v4(); - } - } - - return( Address.is_multicast() || ( Address.to_ulong() == 0xFFFFFFFF ) ); - } - - - void MatrixToQuaternion( const double i_M[9], double (&o_Q)[4]) - { - // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes - // article "Quaternion Calculus and Fast Animation". - - double Trace = i_M[0*3+0]+i_M[1*3+1]+i_M[2*3+2]; - double Root; - - if( Trace > 0 ) - { - // |w| > 1/2, may as well choose w > 1/2 - Root = std::sqrt(Trace + 1.0); // 2w - o_Q[3] = 0.5*Root; - Root = 0.5/Root; // 1/(4w) - o_Q[0] = (i_M[2*3+1]-i_M[1*3+2])*Root; - o_Q[1] = (i_M[0*3+2]-i_M[2*3+0])*Root; - o_Q[2] = (i_M[1*3+0]-i_M[0*3+1])*Root; - } - else - { - // |w| <= 1/2 - int s_Next[3] = { 1, 2, 0 }; - int i = 0; - if ( i_M[1*3+1] > i_M[0*3+0] ) - i = 1; - if ( i_M[2*3+2] > i_M[i*3+i] ) - i = 2; - int j = s_Next[i]; - int k = s_Next[j]; - - Root = std::sqrt(i_M[i*3+i]-i_M[j*3+j]-i_M[k*3+k] + 1.0); - - o_Q[i] = 0.5*Root; - Root = 0.5/Root; - o_Q[3] = (i_M[k*3+j]-i_M[j*3+k])*Root; - o_Q[j] = (i_M[j*3+i]+i_M[i*3+j])*Root; - o_Q[k] = (i_M[k*3+i]+i_M[i*3+k])*Root; - } - - // Normalize - const double InnerProduct = std::inner_product( o_Q, o_Q + 4, o_Q, 0.0 ); - const double Magnitude = std::sqrt( InnerProduct ); - - o_Q[0] /= Magnitude; - o_Q[1] /= Magnitude; - o_Q[2] /= Magnitude; - o_Q[3] /= Magnitude; - } - - - void MatrixToHelical( const double i_rM[9], double (&o_rAA)[3]) - { - double Q[4]; - MatrixToQuaternion( i_rM, Q ); - - const double Real = Q[3]; - - double Len = sqrt( std::inner_product( Q, Q+3, Q, 0.0 ) ); - if( Len > std::numeric_limits< double >::epsilon() * 10 ) - { - const double Angle = 2.0 * atan2( Len, Real ); - const double Scale = Angle / Len; - std::transform( Q, Q+3, o_rAA, [ Scale ]( double X ){ return Scale * X; } ); - } - else - { - std::copy( Q, Q+3, o_rAA ); - } - - } - - void MatrixToEulerXYZ( const double i_M[9], double (&o_rE)[3]) - { - // This original algorithm is the XYZ Euler order and is - // the default input order argument, so no existing code - // should need changing or will give different results - - // Algorithm: GraphicsGems II - Matrix Techniques VII.1 p 320 - - o_rE[1] = asin(i_M[0*3+2]); - - if( fabs( cos(o_rE[1]) ) > std::numeric_limits< double >::epsilon() * 10 ) - { - o_rE[0] = std::atan2(-i_M[1*3+2], i_M[2*3+2]); - o_rE[2] = std::atan2(-i_M[0*3+1], i_M[0*3+0]); - } - else - { - // cos(y) ~= 0 Gimbal-Lock - o_rE[0] = (o_rE[1] > 0) ? std::atan2(i_M[1*3+0], i_M[1*3+1]) : -std::atan2(i_M[0*3+1], i_M[1*3+1]); - o_rE[2] = 0; - } - } - - boost::array< double, 3 > operator*( const boost::array< double, 9 > & i_rM, const boost::array< double, 3 > & i_rX ) - { - boost::array< double, 3 > Result; - - for( size_t i = 0; i < 3; i++) - { - double Sum = 0; - for( unsigned k = 0; k < 3; k++ ) - { - Sum += i_rM[i*3+k] * i_rX[k]; - } - Result[i] = Sum; - } - - return Result; - } - - boost::array< double, 3 > & operator/=( boost::array< double, 3 > & i_rX, double i_Val ) - { - std::transform( i_rX.begin(), i_rX.end(), i_rX.begin(), [ i_Val ]( double x ){ return x / i_Val; } ); - return i_rX; - } - - boost::array< double, 3 > & operator*=( boost::array< double, 3 > & i_rX, double i_Val ) - { - std::transform( i_rX.begin(), i_rX.end(), i_rX.begin(), [ i_Val ]( double x ){ return x * i_Val; } ); - return i_rX; - } - - boost::array< double, 3 > operator+( const boost::array< double, 3 > & i_rX, const boost::array< double, 3 > & i_rY ) - { - boost::array< double, 3 > Result; - std::transform( i_rX.begin(), i_rX.end(), i_rY.begin(), Result.begin(), []( double x, double y ){ return x + y; } ); - return Result; - } - - boost::array< double, 3 > operator-( const boost::array< double, 3 > & i_rX, const boost::array< double, 3 > & i_rY ) - { - boost::array< double, 3 > Result; - std::transform( i_rX.begin(), i_rX.end(), i_rY.begin(), Result.begin(), []( double x, double y ){ return x - y; } ); - return Result; - } - - boost::array< double, 9 > operator*( const boost::array< double, 9 > & i_rM, const boost::array< double, 9 > & i_rN ) - { - boost::array< double, 9 > Result; - - for( size_t i = 0; i < 3; i++) - { - for( size_t j = 0; j < 3; j++) - { - double Sum = 0; - for( unsigned k = 0; k < 3; k++ ) - { - Sum += i_rM[i*3+k] * i_rN[k*3+j]; - } - Result[i*3+j] = Sum; - } - } - - return Result; - } - - - boost::array< double, 9 > Transpose( const boost::array< double, 9 > & i_rM ) - { - boost::array< double, 9 > Result; - - for( size_t i = 0; i < 3; i++) - { - for( size_t j = 0; j < 3; j++) - { - Result[i*3+j] = i_rM[j*3+i]; - } - } - - return Result; - } - - boost::array< double, 16 > Transpose( const boost::array< double, 16 > & i_rM ) - { - boost::array< double, 16 > Result; - - for( size_t i = 0; i < 4; i++) - { - for( size_t j = 0; j < 4; j++) - { - Result[i*4+j] = i_rM[j*4+i]; - } - } - - return Result; - } - - boost::array< double, 3 > CrossProduct( boost::array< double, 3 > & i_rA, boost::array< double, 3 > & i_rB ) - { - boost::array< double, 3 > Result; - Result[0] = i_rA[1] * i_rB[2] - i_rA[2] * i_rB[1]; - Result[1] = i_rA[2] * i_rB[0] - i_rA[0] * i_rB[2]; - Result[2] = i_rA[0] * i_rB[1] - i_rA[1] * i_rB[0]; - return Result; - } - - double DotProduct( boost::array & i_rA, boost::array & i_rB ) - { - double Result = 0; - for( unsigned i = 0; i < 3; ++i ) - { - Result += i_rA[i] * i_rB[i]; - } - return Result; - } -} diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/ClientUtils.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/ClientUtils.h deleted file mode 100644 index a00396a14..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/ClientUtils.h +++ /dev/null @@ -1,118 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - - -#include "Constants.h" - -#include -#include - -namespace ClientUtils -{ - // These functions clear a value - void Clear( bool & o_rValue ); - - void Clear( unsigned int & o_rValue ); - - void Clear( double & o_rValue ); - - void Clear( std::string & o_rValue ); - - void Clear( ViconDataStreamSDK::Core::TimecodeStandard::Enum & o_rValue ); - - void Clear( ViconDataStreamSDK::Core::DeviceType::Enum & o_rValue ); - - void Clear( ViconDataStreamSDK::Core::Unit::Enum & o_rValue ); - - template< typename T, unsigned int N > - void Clear( T (&o_rValue)[ N ] ) - { - for( unsigned int i = 0 ; i < N ; ++i ) - { - Clear( o_rValue[ i ] ); - } - } - - template< typename T > - void Clear( std::vector< T > & o_rValue ) - { - o_rValue.clear(); - } - - // Determine whether an axis is positive or negative - double ComponentSign( ViconDataStreamSDK::Core::Direction::Enum i_Direction ); - - // Determine the index of an enum - unsigned int ComponentIndex( ViconDataStreamSDK::Core::Direction::Enum i_Direction ); - - // Test description of component enum - std::string ComponentName( ViconDataStreamSDK::Core::Direction::Enum i_Direction ); - - std::string AdaptCameraName( const std::string & i_rCameraName, - const std::string & i_rDisplayType, - const unsigned int i_CameraID ); - - std::string AdaptDeviceName( const std::string & i_rDeviceName, - const unsigned int i_DeviceID ); - - std::string AdaptDeviceOutputName( const std::string & i_rDeviceOutputName, - const unsigned int i_DeviceOutputIndex ); - - bool IsValidMulticastIP( const std::string & i_MulticastIPAddress ); - - void MatrixToQuaternion( const double i_M[9], double (&o_Q)[4]); - - void MatrixToHelical( const double i_rM[9], double (&o_rAA)[3]); - - void MatrixToEulerXYZ( const double i_M[9], double (&o_rE)[3]); - - // Insanely simplistic operators for basic vector operations on - // boost arrays. Arrays of length 9 are assumed to be 3x3 matrices. - // Arrays of length 3 are assumed to be 3x1 vectors. - // Only the handful of operations required by Client and AxisMapping have been provided. - - boost::array< double, 3 > operator*( const boost::array< double, 9 > & i_rM, const boost::array< double, 3 > & i_rX ); - - boost::array< double, 3 > & operator/=( boost::array< double, 3 > & i_rX, double i_Val ); - - boost::array< double, 3 > & operator*=( boost::array< double, 3 > & i_rX, double i_Val ); - - boost::array< double, 3 > operator+( const boost::array< double, 3 > & i_rX, double i_Val ); - - boost::array< double, 3 > operator+( const boost::array< double, 3 > & i_rX, const boost::array< double, 3 > & i_rY ); - - boost::array< double, 3 > operator-( const boost::array< double, 3 > & i_rX, const boost::array< double, 3 > & i_rY ); - - boost::array< double, 9 > operator*( const boost::array< double, 9 > & i_rM, const boost::array< double, 9 > & i_rN ); - - boost::array< double, 9 > Transpose( const boost::array< double, 9 > & i_rM ); - - boost::array< double, 16 > Transpose( const boost::array< double, 16 > & i_rM ); - - boost::array< double, 3 > CrossProduct( boost::array< double, 3 > & i_rA, boost::array< double, 3 > & i_rB ); - - double DotProduct( boost::array< double, 3 > & i_rA, boost::array< double, 3 > & i_rB ); -} diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/Constants.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/Constants.h deleted file mode 100644 index ac1f2d9b2..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/Constants.h +++ /dev/null @@ -1,177 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -namespace ViconDataStreamSDK -{ -namespace Core -{ - -namespace Direction -{ - enum Enum - { - Up, - Down, - Left, - Right, - Forward, - Backward - }; -} - -namespace ServerOrientation -{ - enum Enum - { - Unknown, - YUp, - ZUp, - }; -} - -namespace AxisMappingResult -{ - enum Enum - { - Success = 0, - CoLinearAxes, - LeftHandedAxes - }; -} - -namespace StreamMode -{ - enum Enum - { - ClientPull, - ClientPullPreFetch, - ServerPush - }; -} - -namespace TimecodeStandard -{ - enum Enum - { - None, - PAL, - NTSC, - NTSCDrop, - Film, - NTSCFilm, - ATSC - }; -} - -namespace DeviceType -{ - enum Enum - { - Unknown, - ForcePlate, - EyeTracker - }; -}; - -namespace Unit -{ - enum Enum - { - Unknown, - Volt, - Newton, - NewtonMeter, - Meter, - Kilogram, - Second, - Ampere, - Kelvin, - Mole, - Candela, - Radian, - Steradian, - MeterSquared, - MeterCubed, - MeterPerSecond, - MeterPerSecondSquared, - RadianPerSecond, - RadianPerSecondSquared, - Hertz, - Joule, - Watt, - Pascal, - Lumen, - Lux, - Coulomb, - Ohm, - Farad, - Weber, - Tesla, - Henry, - Siemens, - Becquerel, - Gray, - Sievert, - Katal - }; -}; - -namespace Result -{ - enum Enum - { - Unknown, - NotImplemented, - Success, - InvalidHostName, - InvalidMulticastIP, - NullClient, - ClientAlreadyConnected, - ClientConnectionFailed, - ServerAlreadyTransmittingMulticast, - ServerNotTransmittingMulticast, - NotConnected, - NoFrame, - InvalidIndex, - InvalidCameraName, - InvalidSubjectName, - InvalidSegmentName, - InvalidMarkerName, - InvalidDeviceName, - InvalidDeviceOutputName, - InvalidLatencySampleName, - InvalidFrameRateName, - CoLinearAxes, - LeftHandedAxes, - HapticAlreadySet, - EarlyDataRequested, - LateDataRequested, - InvalidOperation - }; -} - -} -} diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/CoreClient.cpp b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/CoreClient.cpp deleted file mode 100644 index ce5b41fc4..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/CoreClient.cpp +++ /dev/null @@ -1,4541 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#include "ClientUtils.h" - -#include "CoreClient.h" - -#include -#include - -#include - -#include "ViconDataStreamSDKCoreVersion.h" - -typedef std::pair< ViconCGStreamType::UInt64, ViconCGStreamType::UInt64 > TPeriod; - - -using namespace ClientUtils; - -namespace -{ - // Timeout for wait-for-frame operations (in milliseconds) - static const unsigned int s_WaitFrameTimeout = 1000; - - // [ Start tick, End tick ) pair. - TPeriod GetFramePeriod( const ViconCGStreamClientSDK::ICGFrameState & i_rFrame ) - { - const ViconCGStreamType::Int64 FramePeriod = i_rFrame.m_Stream.m_FramePeriod; - const ViconCGStreamType::Int64 FrameID = i_rFrame.m_Frame.m_FrameID; - TPeriod Period; - Period.first = FramePeriod * FrameID; - Period.second = Period.first + FramePeriod; - return Period; - } - - /// Calculates the index to the next sample. - /// e.g. given samples | 0 | 1 | 2 | - /// ^ ^ ^ ^ ^ ^ ^ ^ ^ ^ - /// returns 0 1 1 1 2 2 2 3 3 3 - /// \param i_Tick Ticks since sample 0. - /// \param i_SamplePeriod Sample period in ticks. - /// \return The index in device samples - ViconCGStreamType::UInt64 GetNextSampleIndex( const ViconCGStreamType::UInt64 i_SystemTick, - const ViconCGStreamType::UInt64 i_SampleTick, - const ViconCGStreamType::UInt64 i_SamplePeriod ) - { - if( i_SystemTick < i_SampleTick ) - { - return 0; - } - else - { - return ( i_SystemTick - i_SampleTick + i_SamplePeriod - 1 ) / i_SamplePeriod; - } - } - - /// \return the number of samples in the given frame period. - /// Note that we allow samples to overlap the end but not the beginning of the frame period. - /// For instance, given: - /// system frames | A | B | C | D | - /// samples | 0 | 1 | 2 | 3 | 4 | 5 | - /// Frame: samples - /// A: 0, 1 - /// B: 2 - /// C: 3, 4 - /// D: 5 - /// etc. - unsigned int GetSamplesInFrame( const TPeriod & i_rSystemFramePeriod, const ViconCGStreamType::UInt64 i_SampleStartTick, const ViconCGStreamType::UInt64 i_SamplePeriod ) - { - // Order of operations is important here for rounding. - const ViconCGStreamType::UInt64 FirstSampleIndex = GetNextSampleIndex( i_rSystemFramePeriod.first, i_SampleStartTick, i_SamplePeriod ); - const ViconCGStreamType::UInt64 LastSampleIndex = GetNextSampleIndex( i_rSystemFramePeriod.second, i_SampleStartTick, i_SamplePeriod ); - return static_cast< unsigned int >( LastSampleIndex - FirstSampleIndex ); - } - - bool SampleIndexInRange( const unsigned int i_RequestedSample, - const unsigned int i_DeviceFrameID, - const size_t i_SamplesPerDeviceFrame, - const ViconCGStreamType::UInt64 i_SamplePeriod, - const ViconCGStreamType::UInt64 i_SampleTick, - const TPeriod & i_rSystemFramePeriod, - unsigned int & o_rSubSample ) - { - const ViconCGStreamType::UInt64 SamplesStartIndex = i_DeviceFrameID * i_SamplesPerDeviceFrame; - const ViconCGStreamType::UInt64 RequestedSampleIndex = i_RequestedSample + GetNextSampleIndex( i_rSystemFramePeriod.first, i_SampleTick, i_SamplePeriod); - - if( RequestedSampleIndex < SamplesStartIndex || (SamplesStartIndex + i_SamplesPerDeviceFrame) <= RequestedSampleIndex ) - { - return false; - } - - o_rSubSample = static_cast< unsigned int >( RequestedSampleIndex - SamplesStartIndex ); - return true; - } - - const size_t BadIndex = -1; - - const ViconCGStreamType::UInt32 BadFrameValue = -1; - -} - -namespace ViconDataStreamSDK -{ -namespace Core -{ - -VClient::VClient() -: m_ServerPort( 0 ) -, m_bContinuePreFetch( false ) -, m_bNewCachedFrame( false ) -, m_bSegmentDataEnabled( false ) -, m_bMarkerDataEnabled( false ) -, m_bUnlabeledMarkerDataEnabled( false ) -, m_bMarkerRayDataEnabled( false ) -, m_bDeviceDataEnabled( false ) -, m_bCentroidDataEnabled( false ) -, m_bGreyscaleDataEnabled( false ) -, m_bDebugDataEnabled( false ) -, m_bCameraWand2dDataEnabled( false ) -, m_bVideoDataEnabled( false ) -, m_BufferSize( 1 ) -{ - SetAxisMapping( Direction::Forward, Direction::Left, Direction::Up ); - - // set the frame index to a bad value so we know it is not from the stream - m_LatestFrame.m_Frame.m_FrameID = BadFrameValue; - m_CachedFrame = m_LatestFrame; -} - -VClient::~VClient() -{ - Disconnect(); -} - -void VClient::GetVersion( unsigned int & o_rMajor, - unsigned int & o_rMinor, - unsigned int & o_rPoint ) const -{ - // Warning! Don't change these version numbers unless you understand the - // impact of it. They should not change without the approval of Product - // Management. - // * The major version number - When this number increases we break backwards - // compatibility with previous major versions. - // * The minor version number - When this number increases we have probably added - // new functionality to the SDK without breaking backwards - // compatibility with previous versions. - // * The point version number - When this number increases, we have introduced a bug fix - // or performance enhancement without breaking backwards - // compatibility with previous versions. - o_rMajor = VICONDATASTREAMSDKCORE_VERSION_MAJOR; - o_rMinor = VICONDATASTREAMSDKCORE_VERSION_MINOR; - o_rPoint = VICONDATASTREAMSDKCORE_VERSION_POINT; -} - -Result::Enum VClient::Connect( std::shared_ptr< ViconCGStreamClientSDK::ICGClient > i_pClient, - const std::string & i_rHostName ) -{ - if( i_rHostName.length() == 0 ) - { - return Result::InvalidHostName; - } - - if( !i_pClient ) - { - return Result::NullClient; - } - - if( i_pClient->IsConnected() ) - { - return Result::ClientAlreadyConnected; - } - - // Kill any old client - m_pClient.reset(); - - // Parse the supplied host name - std::string HostName = i_rHostName; - unsigned short HostPort = 801; - { - size_t Colon = HostName.rfind( ":" ); - if( Colon != std::string::npos ) - { - try - { - HostPort = boost::lexical_cast< unsigned short >( HostName.substr( Colon+1 ) ); - HostName = HostName.substr( 0, Colon ); - } - catch ( boost::bad_lexical_cast& ) - { - // Revert to unparsed version - HostName = i_rHostName; - HostPort = 801; - } - } - } - - // here we attempt to connect to the IP address - i_pClient->Connect( HostName, HostPort ); - - if( !i_pClient->IsConnected() ) - { - return Result::ClientConnectionFailed; - } - - // copy the pointer if all is well - m_pClient = i_pClient; - m_pClient->SetBufferSize( m_BufferSize ); - m_ServerName = HostName; - m_ServerPort = HostPort; - - // set some default request types - m_pClient->SetRequestTypes( ViconCGStreamEnum::Contents ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::StreamInfo ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::SubjectInfo ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::FrameInfo ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::Timecode ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::LatencyInfo ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::ApplicationInfo ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::SubjectHealth ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::ObjectQuality ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::FrameRateInfo ); - - - // turn everything else off - m_pClient->SetRequestTypes( ViconCGStreamEnum::CameraCalibrationInfo, false); - m_pClient->SetRequestTypes( ViconCGStreamEnum::CameraInfo, false ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::Centroids, false ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::CentroidWeights, false ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::VideoFrame, false ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::LabeledRecons, false ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::UnlabeledRecons, false ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::LocalSegments, false ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::GlobalSegments, false ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::SubjectTopology, false ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::ForceFrame, false ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::MomentFrame, false ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::CentreOfPressureFrame, false ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::VoltageFrame, false ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::GreyscaleBlobs, false ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::EdgePairs, false ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::DeviceInfo, false ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::DeviceInfoExtra, false ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::ForcePlateInfo, false ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::ChannelInfo, false ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::ChannelInfoExtra, false ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::EyeTrackerInfo, false ); - - return Result::Success; -} - -/// Connect to a multicast stream -Result::Enum VClient::ConnectToMulticast( std::shared_ptr< ViconCGStreamClientSDK::ICGClient > i_pClient, - const std::string & i_rLocalIP, - const std::string & i_rMulticastIP ) -{ - if( i_rLocalIP.length() == 0 ) - { - return Result::InvalidHostName; - } - - if( !i_pClient ) - { - return Result::NullClient; - } - - if( i_pClient->IsConnected() || i_pClient->IsMulticastReceiving() ) - { - return Result::ClientAlreadyConnected; - } - - // Parse the supplied multicast ip to extract port if specified - std::string MulticastIP = i_rMulticastIP; - unsigned short MulticastPort = 44801; - { - size_t Colon = MulticastIP.rfind( ":" ); - if( Colon != std::string::npos ) - { - try - { - MulticastPort = boost::lexical_cast< unsigned short >( MulticastIP.substr( Colon+1 ) ); - MulticastIP = MulticastIP.substr( 0, Colon ); - } - catch ( boost::bad_lexical_cast& ) - { - return Result::InvalidMulticastIP; - } - } - } - - if( !IsValidMulticastIP( MulticastIP ) ) - { - return Result::InvalidMulticastIP; - } - - // Kill any old client - m_pClient.reset(); - - // Parse the supplied host name to strip any port (the port cannot be specified) - std::string LocalIP = i_rLocalIP; - { - size_t Colon = LocalIP.rfind( ":" ); - if( Colon != std::string::npos ) - { - LocalIP = LocalIP.substr( 0, Colon ); - } - } - - // here we attempt to connect to the IP address - i_pClient->ReceiveMulticastData( MulticastIP, LocalIP, MulticastPort ); - - if( !i_pClient->IsMulticastReceiving() ) - { - return Result::ClientConnectionFailed; - } - - // copy the pointer if all is well - m_pClient = i_pClient; - m_pClient->SetBufferSize( m_BufferSize ); - - return Result::Success; -} - -/// Disconnect client from the Vicon Data Stream -Result::Enum VClient::Disconnect() -{ - if( !m_pClient ) - { - return Result::NotConnected; - } - - if( !m_pClient->IsConnected() && !m_pClient->IsMulticastReceiving() ) - { - return Result::NotConnected; - } - - EndPreFetchThread(); - - m_pClient.reset(); - m_ServerName.clear(); - m_ServerPort = 0; - - return Result::Success; -} - -Result::Enum VClient::StartTransmittingMulticast( const std::string & i_rServerIP, - const std::string & i_rMulticastIP ) -{ - // Parse the supplied multicast ip to extract port if specified - std::string MulticastIP = i_rMulticastIP; - unsigned short MulticastPort = 44801; - { - size_t Colon = MulticastIP.rfind( ":" ); - if( Colon != std::string::npos ) - { - try - { - MulticastPort = boost::lexical_cast< unsigned short >( MulticastIP.substr( Colon+1 ) ); - MulticastIP = MulticastIP.substr( 0, Colon ); - } - catch ( boost::bad_lexical_cast& ) - { - return Result::InvalidMulticastIP; - } - } - } - - // Parse the supplied host name to strip any port (the port cannot be specified) - std::string HostName = i_rServerIP; - { - size_t Colon = HostName.rfind( ":" ); - if( Colon != std::string::npos ) - { - HostName = HostName.substr( 0, Colon ); - } - } - - if( !m_pClient ) - { - return Result::NotConnected; - } - - if( !m_pClient->IsConnected() ) - { - return Result::NotConnected; - } - - if( !IsValidMulticastIP( MulticastIP ) ) - { - return Result::InvalidMulticastIP; - } - - if( m_pClient->IsMulticastController() ) - { - return Result::ServerAlreadyTransmittingMulticast; - } - - m_pClient->SetServerToTransmitMulticast( MulticastIP, HostName, MulticastPort ); - - return Result::Success; -} - -Result::Enum VClient::StopTransmittingMulticast() -{ - if( !m_pClient ) - { - return Result::NotConnected; - } - - if( !m_pClient->IsConnected() ) - { - return Result::NotConnected; - } - - if( !m_pClient->IsMulticastController() ) - { - return Result::ServerNotTransmittingMulticast; - } - - m_pClient->StopMulticastTransmission(); - - return Result::Success; -} - -void VClient::BeginPreFetchThread() -{ - m_bContinuePreFetch = true; - - // Cache a frame before we start the thread running - FetchNextFrame(); - - if (!m_pPreFetchThread) - { - m_pPreFetchThread.reset( new boost::thread( boost::bind( &VClient::PreFetchThreadBody, this ) ) ); - } -} - -void VClient::EndPreFetchThread() -{ - if( m_pPreFetchThread ) - { - m_bContinuePreFetch = false; - m_pPreFetchThread->join(); - m_pPreFetchThread.reset(); - } -} - -bool VClient::IsConnected() const -{ - if (!m_pClient) - { - return false; - } - - return m_pClient->IsConnected() || m_pClient->IsMulticastReceiving(); -} - -void VClient::SetBufferSize( unsigned int i_MaxFrames ) -{ - m_BufferSize = i_MaxFrames; - if( m_pClient ) - { - m_pClient->SetBufferSize( m_BufferSize ); - } -} - -Result::Enum VClient::GetFrame() -{ - if( !IsConnected() ) - { - return Result::NotConnected; - } - - // Get the next frame - if( !m_pPreFetchThread || !m_bContinuePreFetch ) - { - // Not in pre-fetch mode - FetchNextFrame(); - } - else - { - // In pre-fetch mode - // The pre-fetch thread should have got a frame for us - } - - // Is the latest frame different from the one we dispatched? - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - if( !m_bNewCachedFrame ) - { - return Result::NoFrame; - } - else - { - m_LatestFrame = m_CachedFrame; - m_bNewCachedFrame = false; - return Result::Success; - } -} - -bool VClient::InitGet( Result::Enum & o_rResult ) const -{ - o_rResult = Result::Success; - - if( !IsConnected() ) - { - o_rResult = Result::NotConnected; - } - else if( !HasData() ) - { - o_rResult = Result::NoFrame; - } - - return (o_rResult == Result::Success); -} - -Result::Enum VClient::GetFrameNumber( unsigned int & o_rFrameNumber ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( InitGet( GetResult, o_rFrameNumber ) ) - { - o_rFrameNumber = m_LatestFrame.m_Frame.m_FrameID; - } - - return GetResult; -} - -Result::Enum VClient::GetFrameRate( double & o_rFrameRateInHz ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( InitGet( GetResult, o_rFrameRateInHz ) ) - { - ViconCGStreamType::Int64 Period = m_LatestFrame.m_Stream.m_FramePeriod; - if (Period == 0) - { - o_rFrameRateInHz = 0.0; - } - else - { - o_rFrameRateInHz = static_cast( 135000000 ) / static_cast( Period ); - } - } - - return GetResult; -} - -Result::Enum VClient::GetTimecode( unsigned int & o_rHours, - unsigned int & o_rMinutes, - unsigned int & o_rSeconds, - unsigned int & o_rFrames, - unsigned int & o_rSubFrame, - bool & o_rbFieldFlag, - TimecodeStandard::Enum & o_rTimecodeStandard, - unsigned int & o_rSubFramesPerFrame, - unsigned int & o_rUserBits ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Clear( o_rHours ); - Clear( o_rMinutes ); - Clear( o_rSeconds ); - Clear( o_rFrames ); - Clear( o_rSubFrame ); - Clear( o_rbFieldFlag ); - Clear( o_rTimecodeStandard ); - Clear( o_rSubFramesPerFrame ); - Clear( o_rUserBits ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult ) ) - { - return GetResult; - } - - o_rHours = m_LatestFrame.m_Timecode.m_Hours; - o_rMinutes = m_LatestFrame.m_Timecode.m_Minutes; - o_rSeconds = m_LatestFrame.m_Timecode.m_Seconds; - o_rFrames = m_LatestFrame.m_Timecode.m_Frames; - o_rSubFrame = m_LatestFrame.m_Timecode.m_Subframes; - o_rbFieldFlag = m_LatestFrame.m_Timecode.m_FieldFlag != 0; - o_rSubFramesPerFrame = m_LatestFrame.m_Timecode.m_SubframesPerFrame; - o_rUserBits = m_LatestFrame.m_Timecode.m_UserBits; - - switch( m_LatestFrame.m_Timecode.m_Standard ) - { - case ViconCGStream::VTimecode::ETimecodePAL : o_rTimecodeStandard = TimecodeStandard::PAL; break; - case ViconCGStream::VTimecode::ETimecodeNTSC : o_rTimecodeStandard = TimecodeStandard::NTSC; break; - case ViconCGStream::VTimecode::ETimecodeNTSCDrop : o_rTimecodeStandard = TimecodeStandard::NTSCDrop; break; - case ViconCGStream::VTimecode::ETimecodeFilm : o_rTimecodeStandard = TimecodeStandard::Film; break; - case ViconCGStream::VTimecode::ETimecodeNTSCFilm : o_rTimecodeStandard = TimecodeStandard::NTSCFilm; break; - case ViconCGStream::VTimecode::ETimecodeATSC : o_rTimecodeStandard = TimecodeStandard::ATSC; break; - default: o_rTimecodeStandard = TimecodeStandard::None; break; - } - - return Result::Success; -} - -Result::Enum VClient::GetNetworkLatency(double & o_rLatency) -{ - bool bSuccess = m_pClient->NetworkLatency(o_rLatency); - return bSuccess ? Result::Success : Result::NoFrame; -} - -Result::Enum VClient::GetLatencyTotal( double & o_rLatency ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rLatency ) ) - { - return GetResult; - } - - std::vector< ViconCGStreamDetail::VLatencyInfo_Sample >::const_iterator It = m_LatestFrame.m_Latency.m_Samples.begin(); - std::vector< ViconCGStreamDetail::VLatencyInfo_Sample >::const_iterator End = m_LatestFrame.m_Latency.m_Samples.end(); - for( ; It != End ; ++It ) - { - o_rLatency += It->m_Latency; - } - - return Result::Success; -} - -Result::Enum VClient::GetLatencySampleCount( unsigned int & o_rSampleCount ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( InitGet( GetResult, o_rSampleCount ) ) - { - o_rSampleCount = static_cast< unsigned int >( m_LatestFrame.m_Latency.m_Samples.size() ); - } - - return GetResult; -} - -Result::Enum VClient::GetLatencySampleName( const unsigned int i_SampleIndex, std::string & o_rSampleName ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rSampleName ) ) - { - return GetResult; - } - - if( i_SampleIndex >= m_LatestFrame.m_Latency.m_Samples.size() ) - { - return Result::InvalidIndex; - } - - o_rSampleName = m_LatestFrame.m_Latency.m_Samples[ i_SampleIndex ].m_Name; - - return Result::Success; -} - -Result::Enum VClient::GetLatencySampleValue( const std::string & i_rSampleName, double & o_rSampleValue ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rSampleValue ) ) - { - return GetResult; - } - - std::vector< ViconCGStreamDetail::VLatencyInfo_Sample >::const_iterator It = m_LatestFrame.m_Latency.m_Samples.begin(); - std::vector< ViconCGStreamDetail::VLatencyInfo_Sample >::const_iterator End = m_LatestFrame.m_Latency.m_Samples.end(); - for( ; It != End ; ++It ) - { - if( It->m_Name == i_rSampleName ) - { - o_rSampleValue = It->m_Latency; - return Result::Success; - } - } - - return Result::InvalidLatencySampleName; -} - -Result::Enum VClient::GetHardwareFrameNumber( unsigned int & o_rFrameNumber ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( InitGet( GetResult, o_rFrameNumber ) ) - { - o_rFrameNumber = m_LatestFrame.m_HardwareFrame.m_HardwareFrame; - } - - return GetResult; -} - -Result::Enum VClient::GetFrameRateCount( unsigned int & o_rFrameRateCount ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rFrameRateCount ) ) - { - return GetResult; - } - - o_rFrameRateCount = static_cast< unsigned int >( m_LatestFrame.m_FrameRateInfo.m_FrameRates.size() ); - return Result::Success; -} - -Result::Enum VClient::GetFrameRateName( const unsigned int i_FrameRateIndex, std::string & o_rFrameRateName ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rFrameRateName ) ) - { - return GetResult; - } - - if( i_FrameRateIndex >= m_LatestFrame.m_FrameRateInfo.m_FrameRates.size() ) - { - return Result::InvalidIndex; - } - - unsigned int Counter = 0; - std::map< std::string, double >::const_iterator It= m_LatestFrame.m_FrameRateInfo.m_FrameRates.begin(); - std::map< std::string, double >::const_iterator End= m_LatestFrame.m_FrameRateInfo.m_FrameRates.end(); - for( ; It!=End; ++It, ++Counter ) - { - if( Counter == i_FrameRateIndex ) - { - o_rFrameRateName = It->first; - break; - } - } - return Result::Success; -} - -Result::Enum VClient::GetFrameRateValue( const std::string & i_rFrameRateName, double & o_rFrameRateValue ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rFrameRateValue ) ) - { - return GetResult; - } - - if( !m_LatestFrame.m_FrameRateInfo.m_FrameRates.count( i_rFrameRateName ) ) - { - return Result::InvalidFrameRateName; - } - - - std::map FrameRates = m_LatestFrame.m_FrameRateInfo.m_FrameRates; - o_rFrameRateValue = FrameRates[i_rFrameRateName]; - return Result::Success; -} - -Result::Enum VClient::SetSegmentDataEnabled( const bool i_bEnabled ) -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - if( !IsConnected() ) - { - return Result::NotConnected; - } - - m_pClient->SetRequestTypes( ViconCGStreamEnum::LocalSegments, i_bEnabled ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::GlobalSegments, i_bEnabled ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::SubjectTopology, i_bEnabled ); - - m_bSegmentDataEnabled = i_bEnabled; - return Result::Success; -} - -Result::Enum VClient::SetMarkerDataEnabled( const bool i_bEnabled ) -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - if( !IsConnected() ) - { - return Result::NotConnected; - } - - m_pClient->SetRequestTypes( ViconCGStreamEnum::LabeledRecons, i_bEnabled ); - - m_bMarkerDataEnabled = i_bEnabled; - return Result::Success; -} - -Result::Enum VClient::SetUnlabeledMarkerDataEnabled( const bool i_bEnabled ) -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - if( !IsConnected() ) - { - return Result::NotConnected; - } - - m_pClient->SetRequestTypes( ViconCGStreamEnum::UnlabeledRecons, i_bEnabled ); - - m_bUnlabeledMarkerDataEnabled = i_bEnabled; - return Result::Success; -} - -Result::Enum VClient::SetMarkerRayDataEnabled( const bool i_bEnabled ) -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - if( !IsConnected() ) - { - return Result::NotConnected; - } - - m_pClient->SetRequestTypes( ViconCGStreamEnum::LabeledReconRayAssignments, i_bEnabled ); - - m_bMarkerRayDataEnabled = i_bEnabled; - return Result::Success; -} - -Result::Enum VClient::SetDeviceDataEnabled( const bool i_bEnabled ) -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - if( !IsConnected() ) - { - return Result::NotConnected; - } - - m_pClient->SetRequestTypes( ViconCGStreamEnum::DeviceInfo, i_bEnabled ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::DeviceInfoExtra, i_bEnabled ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::ChannelInfo, i_bEnabled ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::ChannelInfoExtra, i_bEnabled ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::ForcePlateInfo, i_bEnabled ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::EyeTrackerInfo, i_bEnabled ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::ForceFrame, i_bEnabled ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::MomentFrame, i_bEnabled ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::CentreOfPressureFrame, i_bEnabled ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::VoltageFrame, i_bEnabled ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::EyeTrackerFrame, i_bEnabled ); - - m_bDeviceDataEnabled = i_bEnabled; - return Result::Success; -} - -Result::Enum VClient::SetCentroidDataEnabled( const bool i_bEnabled ) -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - if( !IsConnected() ) - { - return Result::NotConnected; - } - - m_pClient->SetRequestTypes( ViconCGStreamEnum::CameraInfo, i_bEnabled ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::Centroids, i_bEnabled ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::CentroidWeights, i_bEnabled ); - - m_bCentroidDataEnabled = i_bEnabled; - return Result::Success; - -} - -Result::Enum VClient::SetGreyscaleDataEnabled( const bool i_bEnabled ) -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - if( !IsConnected() ) - { - return Result::NotConnected; - } - - m_pClient->SetRequestTypes( ViconCGStreamEnum::CameraInfo, i_bEnabled ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::GreyscaleBlobs, i_bEnabled ); - - m_bGreyscaleDataEnabled = i_bEnabled; - return Result::Success; -} - -Result::Enum VClient::SetDebugDataEnabled( const bool i_bEnabled ) -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - if( !IsConnected() ) - { - return Result::NotConnected; - } - - m_pClient->SetRequestTypes( ViconCGStreamEnum::HardwareFrameInfo, i_bEnabled ); - m_bDebugDataEnabled = i_bEnabled; - return Result::Success; -} - -Result::Enum VClient::SetCameraWand2dDataEnabled( const bool i_bEnabled ) -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - if( !IsConnected() ) - { - return Result::NotConnected; - } - - m_pClient->SetRequestTypes( ViconCGStreamEnum::CameraInfo, i_bEnabled ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::CameraWand2d, i_bEnabled ); - m_bCameraWand2dDataEnabled = i_bEnabled; - return Result::Success; -} - -Result::Enum VClient::SetVideoDataEnabled( const bool i_bEnabled ) -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - if( !IsConnected() ) - { - return Result::NotConnected; - } - - m_pClient->SetRequestTypes( ViconCGStreamEnum::CameraInfo, i_bEnabled ); - m_pClient->SetRequestTypes( ViconCGStreamEnum::VideoFrame, i_bEnabled ); - - m_bVideoDataEnabled = i_bEnabled; - return Result::Success; - -} - -bool VClient::IsSegmentDataEnabled() const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - return m_bSegmentDataEnabled; -} - -bool VClient::IsMarkerDataEnabled() const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - return m_bMarkerDataEnabled; -} - -bool VClient::IsUnlabeledMarkerDataEnabled() const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - return m_bUnlabeledMarkerDataEnabled; -} - -bool VClient::IsMarkerRayDataEnabled() const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - return m_bMarkerRayDataEnabled; -} - -bool VClient::IsDeviceDataEnabled() const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - return m_bDeviceDataEnabled; -} - -bool VClient::IsCentroidDataEnabled() const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - return m_bCentroidDataEnabled; -} - -bool VClient::IsGreyscaleDataEnabled() const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - return m_bGreyscaleDataEnabled; -} - -bool VClient::IsVideoDataEnabled() const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - return m_bVideoDataEnabled; -} - -bool VClient::IsDebugDataEnabled() const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - return m_bDebugDataEnabled; -} - -bool VClient::IsCameraWand2dDataEnabled() const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - return m_bCameraWand2dDataEnabled; -} - - -Result::Enum VClient::SetStreamMode( const StreamMode::Enum i_Mode ) -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - if( !IsConnected() ) - { - return Result::NotConnected; - } - - switch( i_Mode ) - { - case StreamMode::ServerPush : EndPreFetchThread(); - m_pClient->SetStreamMode( true ); - break; - case StreamMode::ClientPullPreFetch : m_pClient->SetStreamMode( false ); - BeginPreFetchThread(); - break; - case StreamMode::ClientPull : - default : m_pClient->SetStreamMode( false ); - EndPreFetchThread(); - break; - } - - return Result::Success; -} - -Result::Enum VClient::SetApexDeviceFeedback( const std::string& i_rDeviceName, bool i_bOn ) -{ - if( !IsConnected() ) - { - return Result::NotConnected; - } - if( !HasData() ) - { - return Result::NoFrame; - } - - unsigned int DeviceID; - if( GetDeviceID( i_rDeviceName, DeviceID )!= Result::Success ) - { - return Result::InvalidDeviceName; - } - - if( !m_pClient ) - { - return Result::NullClient; - } - - if( m_pClient->SetApexDeviceFeedback( DeviceID, i_bOn ) ) - { - return Result::Success; - } - - return Result::HapticAlreadySet; - -} - -Result::Enum VClient::SetAxisMapping( const Direction::Enum i_XAxis, - const Direction::Enum i_YAxis, - const Direction::Enum i_ZAxis ) -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - AxisMappingResult::Enum Result; - std::shared_ptr< VAxisMapping > pAxisMapping = VAxisMapping::Create( Result, i_XAxis, i_YAxis, i_ZAxis ); - - if( ! pAxisMapping ) - { - switch( Result ) - { - case AxisMappingResult::CoLinearAxes: return Result::CoLinearAxes; - case AxisMappingResult::LeftHandedAxes: return Result::LeftHandedAxes; - default: return Result::Unknown; - } - } - - m_pAxisMapping = pAxisMapping; - - return Result::Success; -} - -void VClient::GetAxisMapping( Direction::Enum & o_rXAxis, Direction::Enum & o_rYAxis, Direction::Enum & o_rZAxis ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - m_pAxisMapping->GetAxisMapping( o_rXAxis, o_rYAxis, o_rZAxis ); -} - -Result::Enum VClient::GetServerOrientation( ServerOrientation::Enum & o_rServerOrientation ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - if( m_CachedFrame.m_ApplicationInfo ) - { - switch(m_CachedFrame.m_ApplicationInfo.get().m_AxisOrientation ) - { - case ViconCGStream::VApplicationInfo::EYUp: - o_rServerOrientation = ServerOrientation::YUp; - break; - case ViconCGStream::VApplicationInfo::EZUp: - o_rServerOrientation = ServerOrientation::ZUp; - break; - default: - o_rServerOrientation=ServerOrientation::Unknown; - break; - } - return Result::Success; - } - else - { - o_rServerOrientation=ServerOrientation::Unknown; - return Result::NotImplemented; - } - -} - -Result::Enum VClient::GetSubjectCount( unsigned int & o_rSubjectCount ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( InitGet( GetResult, o_rSubjectCount ) ) - { - o_rSubjectCount = static_cast< unsigned int >( m_LatestFrame.m_Subjects.size() ); - } - - return GetResult; -} - -Result::Enum VClient::GetSubjectName( const unsigned int i_SubjectIndex, - std::string& o_rSubjectName ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rSubjectName ) ) - { - return GetResult; - } - - if( i_SubjectIndex >= m_LatestFrame.m_Subjects.size() ) - { - return Result::InvalidIndex; - } - - o_rSubjectName = m_LatestFrame.m_Subjects[ i_SubjectIndex ].m_Name; - return Result::Success; -} - -Result::Enum VClient::GetSubjectRootSegmentName( const std::string & i_rSubjectName, - std::string & o_rSegmentName ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Clear( o_rSegmentName ); - - // here we have a valid frame of data. need to check for this subject and retrieve information - Result::Enum GetResult = Result::Success; - const ViconCGStream::VSubjectInfo * pSubjectInfo = GetSubjectInfo( i_rSubjectName, GetResult ); - if( !pSubjectInfo ) - { - return GetResult; - } - - std::vector< ViconCGStreamDetail::VSubjectInfo_Segment >::const_iterator SegIt = pSubjectInfo->m_Segments.begin(); - std::vector< ViconCGStreamDetail::VSubjectInfo_Segment >::const_iterator SegEnd = pSubjectInfo->m_Segments.end(); - for( ; SegIt != SegEnd ; ++SegIt ) - { - if( SegIt->m_ParentID == 0 ) - { - o_rSegmentName = SegIt->m_Name; - return Result::Success; - } - } - - return Result::Unknown; -} - -Result::Enum VClient::GetSegmentCount( const std::string & i_rSubjectName, - unsigned int & o_rSegmentCount ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Clear( o_rSegmentCount ); - - Result::Enum GetResult = Result::Success; - // here we have a valid frame of data. need to check for this subject and retrieve information - const ViconCGStream::VSubjectInfo * pSubjectInfo = GetSubjectInfo( i_rSubjectName, GetResult ); - if( pSubjectInfo ) - { - o_rSegmentCount = static_cast< unsigned int >( pSubjectInfo->m_Segments.size() ); - return Result::Success; - } - - return GetResult; -} - -Result::Enum VClient::GetSegmentName( const std::string & i_rSubjectName, - const unsigned int i_SegmentIndex, - std::string & o_rSegmentName ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Clear( o_rSegmentName ); - - Result::Enum GetResult = Result::Success; - const ViconCGStream::VSubjectInfo * pSubjectInfo = GetSubjectInfo( i_rSubjectName, GetResult ); - if( pSubjectInfo ) - { - if( i_SegmentIndex >= pSubjectInfo->m_Segments.size() ) - { - return Result::InvalidIndex; - } - - o_rSegmentName = pSubjectInfo->m_Segments[ i_SegmentIndex ].m_Name; - return Result::Success; - } - - return GetResult; -} - -Result::Enum VClient::GetSegmentChildCount( const std::string & i_rSubjectName, - const std::string & i_rSegmentName, - unsigned int & o_rSegmentCount ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rSegmentCount ) ) - { - return GetResult; - } - - unsigned int SubjectID = 0; - unsigned int SegmentID = 0; - { - Result::Enum result = GetSubjectAndSegmentID( i_rSubjectName, i_rSegmentName, SubjectID, SegmentID ); - if( result != Result::Success ) - { - return result; - } - } - - // here we have a valid frame of data. need to check for this subject and retrieve information - std::vector< ViconCGStream::VSubjectInfo >::const_iterator SubIt = m_LatestFrame.m_Subjects.begin(); - std::vector< ViconCGStream::VSubjectInfo >::const_iterator SubEnd = m_LatestFrame.m_Subjects.end(); - for( ; SubIt != SubEnd ; ++SubIt ) - { - if( SubjectID == SubIt->m_SubjectID ) - { - unsigned int ChildCount = 0; - std::vector< ViconCGStreamDetail::VSubjectInfo_Segment >::const_iterator SegIt = SubIt->m_Segments.begin(); - std::vector< ViconCGStreamDetail::VSubjectInfo_Segment >::const_iterator SegEnd = SubIt->m_Segments.end(); - for( ; SegIt != SegEnd ; ++SegIt ) - { - if( SegIt->m_ParentID == SegmentID ) - { - ++ChildCount; - } - } - - o_rSegmentCount = ChildCount; - return Result::Success; - } - } - - return Result::NotImplemented; -} - -Result::Enum VClient::GetSegmentChildName( const std::string& i_rSubjectName, const std::string& i_rSegmentName, unsigned int i_SegmentIndex, std::string& o_rSegmentName ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rSegmentName ) ) - { - return GetResult; - } - - unsigned int SubjectID = 0; - unsigned int SegmentID = 0; - { - Result::Enum result = GetSubjectAndSegmentID( i_rSubjectName, i_rSegmentName, SubjectID, SegmentID ); - if( result != Result::Success ) - { - return result; - } - } - - // here we have a valid frame of data. need to check for this subject and retrieve information - std::vector< ViconCGStream::VSubjectInfo >::const_iterator SubIt = m_LatestFrame.m_Subjects.begin(); - std::vector< ViconCGStream::VSubjectInfo >::const_iterator SubEnd = m_LatestFrame.m_Subjects.end(); - for( ; SubIt != SubEnd ; ++SubIt ) - { - if( SubjectID == SubIt->m_SubjectID ) - { - unsigned int ChildCount = 0; - std::vector< ViconCGStreamDetail::VSubjectInfo_Segment >::const_iterator SegIt = SubIt->m_Segments.begin(); - std::vector< ViconCGStreamDetail::VSubjectInfo_Segment >::const_iterator SegEnd = SubIt->m_Segments.end(); - for( ; SegIt != SegEnd ; ++SegIt ) - { - if( SegIt->m_ParentID == SegmentID ) - { - if( ChildCount == i_SegmentIndex ) - { - o_rSegmentName = SegIt->m_Name; - return Result::Success; - } - else - { - ++ChildCount; - } - } - } - return Result::InvalidIndex; - } - } - - return Result::NotImplemented; -} - -Result::Enum VClient::GetSegmentParentName( const std::string& i_rSubjectName, const std::string& i_rSegmentName, std::string& o_rSegmentName ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rSegmentName ) ) - { - return GetResult; - } - - unsigned int SubjectID = 0; - unsigned int SegmentID = 0; - { - Result::Enum result = GetSubjectAndSegmentID( i_rSubjectName, i_rSegmentName, SubjectID, SegmentID ); - if( result != Result::Success ) - { - return result; - } - } - - // here we have a valid frame of data. need to check for this subject and retrieve information - std::vector< ViconCGStream::VSubjectInfo >::const_iterator SubIt = m_LatestFrame.m_Subjects.begin(); - std::vector< ViconCGStream::VSubjectInfo >::const_iterator SubEnd = m_LatestFrame.m_Subjects.end(); - for( ; SubIt != SubEnd ; ++SubIt ) - { - if( SubjectID == SubIt->m_SubjectID ) - { - unsigned int ParentID = 0; - { - std::vector< ViconCGStreamDetail::VSubjectInfo_Segment >::const_iterator SegIt = SubIt->m_Segments.begin(); - std::vector< ViconCGStreamDetail::VSubjectInfo_Segment >::const_iterator SegEnd = SubIt->m_Segments.end(); - for( ; SegIt != SegEnd ; ++SegIt ) - { - if( SegIt->m_SegmentID == SegmentID ) - { - ParentID = SegIt->m_ParentID; - break; - } - } - } - - // Get the parent name from the ID - { - std::vector< ViconCGStreamDetail::VSubjectInfo_Segment >::const_iterator SegIt = SubIt->m_Segments.begin(); - std::vector< ViconCGStreamDetail::VSubjectInfo_Segment >::const_iterator SegEnd = SubIt->m_Segments.end(); - for( ; SegIt != SegEnd ; ++SegIt ) - { - if( SegIt->m_SegmentID == ParentID ) - { - o_rSegmentName = SegIt->m_Name; - return Result::Success; - } - } - - return Result::Unknown; - } - } - } - - return Result::NotImplemented; -} - -Result::Enum VClient::GetSegmentStaticTranslation( const std::string& i_rSubjectName, - const std::string& i_rSegmentName, - double (&o_rThreeVector)[3] ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rThreeVector ) ) - { - return GetResult; - } - - unsigned int SubjectID = 0; - unsigned int SegmentID = 0; - { - Result::Enum result = GetSubjectAndSegmentID( i_rSubjectName, i_rSegmentName, SubjectID, SegmentID ); - if( result != Result::Success ) - { - return result; - } - } - - const ViconCGStream::VSubjectTopology * pSubjectTopology = GetSubjectTopology( SubjectID ); - if( !pSubjectTopology ) - { - return Result::InvalidSubjectName; - } - - std::vector< ViconCGStreamDetail::VSubjectTopology_Segment >::const_iterator It = pSubjectTopology->m_Segments.begin(); - std::vector< ViconCGStreamDetail::VSubjectTopology_Segment >::const_iterator End = pSubjectTopology->m_Segments.end(); - for( ; It != End ; ++It ) - { - if( It->m_SegmentID == SegmentID ) - { - CopyAndTransformT( It->m_Translation, o_rThreeVector ); - return Result::Success; - } - } - - return Result::InvalidSegmentName; -} - -Result::Enum VClient::GetObjectQuality( const std::string& i_rObjectName, double& o_rQuality ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - - // here we have a valid frame of data. first check whether we have a valid subject - const ViconCGStream::VSubjectInfo * pSubjectInfo = GetSubjectInfo( i_rObjectName, GetResult ); - if( pSubjectInfo ) - { - // See if we have an object quality associated with the subject ID. - const ViconCGStream::VObjectQuality * pObjectQuality = GetObjectQuality( pSubjectInfo->m_SubjectID ); - if( !pObjectQuality ) - { - // Not all servers will support ObjectQuality. Currently only Tracker. - return Result::NotImplemented; - } - - o_rQuality = pObjectQuality->m_ObjectQuality; - return Result::Success; - } - - return GetResult; -} - -Result::Enum VClient::GetMarkerCount( const std::string & i_rSubjectName, - unsigned int & o_rMarkerCount ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - // here we have a valid frame of data. need to check for this subject and retrieve information - const ViconCGStream::VSubjectInfo * pSubjectInfo = GetSubjectInfo( i_rSubjectName, GetResult ); - if( pSubjectInfo ) - { - o_rMarkerCount = static_cast< unsigned int >( pSubjectInfo->m_Markers.size() ); - } - - return GetResult; -} - -Result::Enum VClient::GetMarkerName( const std::string & i_rSubjectName, - const unsigned int i_MarkerIndex, - std::string & o_rMarkerName ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Clear( o_rMarkerName ); - - Result::Enum GetResult = Result::Success; - const ViconCGStream::VSubjectInfo * pSubjectInfo = GetSubjectInfo( i_rSubjectName, GetResult ); - if( pSubjectInfo ) - { - // check that index is correct - if( i_MarkerIndex >= pSubjectInfo->m_Markers.size() ) - { - return Result::InvalidIndex; - } - - o_rMarkerName = pSubjectInfo->m_Markers[ i_MarkerIndex ].m_Name; - } - - return GetResult; -} - -Result::Enum VClient::GetMarkerParentName( const std::string & i_rSubjectName, const std::string & i_rMarkerName, std::string & o_rSegmentName ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Clear( o_rSegmentName ); - - Result::Enum GetResult = Result::Success; - const ViconCGStream::VSubjectInfo * pSubjectInfo = GetSubjectInfo( i_rSubjectName, GetResult ); - if ( !pSubjectInfo ) - { - return GetResult; - } - - unsigned int MarkerID = 0; - GetResult = GetMarkerID( *pSubjectInfo, i_rMarkerName, MarkerID ); - if ( GetResult != Result::Success ) - { - return GetResult; - } - - unsigned int SegmentID = 0; - - std::vector< ViconCGStreamDetail::VSubjectInfo_Attachment >::const_iterator AttachIt = pSubjectInfo->m_Attachments.begin(); - std::vector< ViconCGStreamDetail::VSubjectInfo_Attachment >::const_iterator AttachEnd = pSubjectInfo->m_Attachments.end(); - for( ; AttachIt != AttachEnd ; ++AttachIt ) - { - if( AttachIt->m_MarkerID == MarkerID ) - { - SegmentID = AttachIt->m_SegmentID; - break; - } - } - - std::vector< ViconCGStreamDetail::VSubjectInfo_Segment >::const_iterator SegmentIt = pSubjectInfo->m_Segments.begin(); - std::vector< ViconCGStreamDetail::VSubjectInfo_Segment >::const_iterator SegmentEnd = pSubjectInfo->m_Segments.end(); - for( ; SegmentIt != SegmentEnd ; ++SegmentIt ) - { - if( SegmentIt->m_SegmentID == SegmentID ) - { - o_rSegmentName = SegmentIt->m_Name; - return Result::Success; - } - } - - return Result::InvalidMarkerName; -} - -Result::Enum VClient::GetMarkerGlobalTranslation( const std::string & i_rSubjectName, - const std::string & i_rMarkerName, - double (& o_rThreeVector)[3], - bool & o_rbOccludedFlag ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rThreeVector, o_rbOccludedFlag ) ) - { - return GetResult; - } - - unsigned int SubjectID = 0; - unsigned int MarkerID = 0; - Result::Enum _Result = GetSubjectAndMarkerID( i_rSubjectName, i_rMarkerName, SubjectID, MarkerID ); - if( Result::Success != _Result ) - { - return _Result; - } - - // go through the frame's reconstructions and find its position in this frame - for( unsigned int i = 0 ; i < m_LatestFrame.m_LabeledRecons.m_LabeledRecons.size() ; ++i ) - { - const ViconCGStreamDetail::VLabeledRecons_LabeledRecon& rRecon = m_LatestFrame.m_LabeledRecons.m_LabeledRecons[i]; - if( rRecon.m_SubjectID == SubjectID && rRecon.m_MarkerID == MarkerID ) - { - CopyAndTransformT( rRecon.m_Position, o_rThreeVector ); - return Result::Success; - } - } - - // if we're here then they entered correct subject and marker info, - // but the data was not in the frame. This is okay but we must - // set the occluded flag and return true - o_rbOccludedFlag = true; - return Result::Success; -} - -Result::Enum VClient::GetMarkerRayAssignmentCount( const std::string& i_rSubjectName, const std::string& i_rMarkerName, unsigned int &o_rAssignmentCount ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if( !InitGet( GetResult, o_rAssignmentCount ) ) - { - return GetResult; - } - - std::vector< unsigned int > RayAssignments; - std::vector< unsigned int > CentroidIndices; - GetResult = GetReconRayAssignments( i_rSubjectName, i_rMarkerName, RayAssignments, CentroidIndices ); - if( GetResult == Result::Success ) - { - o_rAssignmentCount = static_cast< unsigned int >( RayAssignments.size() ); - } - - return GetResult; -} - -Result::Enum VClient::GetMarkerRayAssignment( const std::string& i_rSubjectName, const std::string& i_rMarkerName, int i_AssignmentIndex, unsigned int & o_rCameraID, unsigned int & o_rCentroidIndex ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if( !InitGet( GetResult, o_rCameraID ) ) - { - return GetResult; - } - - std::vector< unsigned int > RayAssignments; - std::vector< unsigned int > CentroidIndices; - GetResult = GetReconRayAssignments( i_rSubjectName, i_rMarkerName, RayAssignments, CentroidIndices ); - if( GetResult == Result::Success ) - { - if( i_AssignmentIndex < static_cast< int >( RayAssignments.size() ) ) - { - o_rCameraID = RayAssignments[i_AssignmentIndex]; - o_rCentroidIndex = CentroidIndices[ i_AssignmentIndex ]; - return Result::Success; - } - return Result::InvalidIndex; - } - - return GetResult; -} - -Result::Enum VClient::GetUnlabeledMarkerCount( unsigned int & o_rMarkerCount ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( InitGet( GetResult, o_rMarkerCount ) ) - { - o_rMarkerCount = static_cast< unsigned int >( m_LatestFrame.m_UnlabeledRecons.m_UnlabeledRecons.size() ); - } - - return GetResult; -} - -Result::Enum VClient::GetUnlabeledMarkerGlobalTranslation( const unsigned int i_MarkerIndex, - double (& o_rTranslation)[3] ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rTranslation ) ) - { - return GetResult; - } - - if( i_MarkerIndex >= m_LatestFrame.m_UnlabeledRecons.m_UnlabeledRecons.size() ) - { - return Result::InvalidIndex; - } - - CopyAndTransformT( m_LatestFrame.m_UnlabeledRecons.m_UnlabeledRecons[ i_MarkerIndex ].m_Position, o_rTranslation ); - return Result::Success; -} - -Result::Enum VClient::GetLabeledMarkerCount( unsigned int & o_rMarkerCount ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( InitGet( GetResult, o_rMarkerCount ) ) - { - o_rMarkerCount = static_cast< unsigned int >( m_LatestFrame.m_LabeledRecons.m_LabeledRecons.size() ); - } - - return GetResult; -} - -Result::Enum VClient::GetLabeledMarkerGlobalTranslation( const unsigned int i_MarkerIndex, - double (& o_rTranslation)[3] ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rTranslation ) ) - { - return GetResult; - } - - if( i_MarkerIndex >= m_LatestFrame.m_LabeledRecons.m_LabeledRecons.size() ) - { - return Result::InvalidIndex; - } - - CopyAndTransformT( m_LatestFrame.m_LabeledRecons.m_LabeledRecons[ i_MarkerIndex ].m_Position, o_rTranslation ); - return Result::Success; -} - -Result::Enum VClient::GetSegmentGlobalTranslation( const std::string & i_rSubjectName, - const std::string & i_rSegmentName, - double ( & o_rThreeVector )[3], - bool & o_rbOccludedFlag ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rThreeVector, o_rbOccludedFlag ) ) - { - return GetResult; - } - - // Look up the ids for the subject and segment - unsigned int SubjectID = 0; - unsigned int SegmentID = 0; - Result::Enum _Result = GetSubjectAndSegmentID( i_rSubjectName, i_rSegmentName, SubjectID, SegmentID ); - if( Result::Success != _Result ) - { - return _Result; - } - - // go through the frame's segment data and find its position in this frame - for (unsigned int i = 0; i < m_LatestFrame.m_GlobalSegments.size(); i++) - { - const ViconCGStream::VGlobalSegments& rSegments = m_LatestFrame.m_GlobalSegments[i]; - if (rSegments.m_SubjectID == SubjectID) - { - // now look through its segments - for (unsigned int j = 0; j < rSegments.m_Segments.size(); j++) - { - const ViconCGStreamDetail::VGlobalSegments_Segment& rSegment = rSegments.m_Segments[j]; - if (rSegment.m_SegmentID == SegmentID) - { - CopyAndTransformT( rSegment.m_Translation, o_rThreeVector ); - return Result::Success; - } - } - } - } - - // if we're here then they entered correct subject and marker info, - // but the data was not in the frame. This is okay but we must - // set the occluded flag and return true - o_rbOccludedFlag = true; - return Result::Success; -} - -Result::Enum VClient::GetSegmentGlobalRotationHelical( const std::string & i_rSubjectName, - const std::string & i_rSegmentName, - double ( & o_rThreeVector)[3], - bool & o_rbOccluded ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Clear( o_rThreeVector ); - Clear( o_rbOccluded ); - // InitGet is called by GetSegmentGlobalRotationMatrix. - - // Get the answer as a rotation matrix - double RotationArray[ 9 ]; - const Result::Enum _Result = GetSegmentGlobalRotationMatrix( i_rSubjectName, i_rSegmentName, RotationArray, o_rbOccluded ); - - if( Result::Success == _Result && !o_rbOccluded ) - { - MatrixToHelical( RotationArray, o_rThreeVector ); - } - - return _Result; -} - -Result::Enum VClient::GetSegmentGlobalRotationMatrix( const std::string & i_rSubjectName, - const std::string & i_rSegmentName, - double (& o_rRotation)[9], - bool & o_rbOccluded ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rRotation, o_rbOccluded ) ) - { - return GetResult; - } - - // Look up the ids for the subject and segment - unsigned int SubjectID = 0; - unsigned int SegmentID = 0; - Result::Enum _Result = GetSubjectAndSegmentID( i_rSubjectName, i_rSegmentName, SubjectID, SegmentID ); - if( Result::Success != _Result ) - { - return _Result; - } - - // go through the frame's segment data and find its position in this frame - std::vector< ViconCGStream::VGlobalSegments >::const_iterator SegmentIt = m_LatestFrame.m_GlobalSegments.begin(); - std::vector< ViconCGStream::VGlobalSegments >::const_iterator SegmentEnd = m_LatestFrame.m_GlobalSegments.end(); - for( ; SegmentIt != SegmentEnd ; ++SegmentIt ) - { - const ViconCGStream::VGlobalSegments & rSegments( *SegmentIt ); - if( rSegments.m_SubjectID == SubjectID ) - { - // now look through the segment detail - std::vector< ViconCGStreamDetail::VGlobalSegments_Segment >::const_iterator SegmentDetailIt = rSegments.m_Segments.begin(); - std::vector< ViconCGStreamDetail::VGlobalSegments_Segment >::const_iterator SegmentDetailEnd = rSegments.m_Segments.end(); - for( ; SegmentDetailIt != SegmentDetailEnd ; ++SegmentDetailIt ) - { - const ViconCGStreamDetail::VGlobalSegments_Segment & rSegment( *SegmentDetailIt ); - if( rSegment.m_SegmentID == SegmentID ) - { - // copy out the answer - CopyAndTransformR( rSegment.m_Rotation, o_rRotation ); - o_rbOccluded = false; - return Result::Success; - } - } - } - } - - // if we're here then they entered correct subject and marker info, - // but the data was not in the frame. This is okay but we must - // set the occluded flag and return true - o_rbOccluded = true; - return Result::Success; -} - -Result::Enum VClient::GetSegmentGlobalRotationQuaternion( const std::string & i_rSubjectName, - const std::string & i_rSegmentName, - double ( & o_rFourVector)[4], - bool & o_rbOccluded ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Clear( o_rFourVector ); - Clear( o_rbOccluded ); - // InitGet is called by GetSegmentGlobalRotationMatrix. - - // Get the answer as a rotation matrix - double RotationArray[ 9 ]; - const Result::Enum _Result = GetSegmentGlobalRotationMatrix( i_rSubjectName, i_rSegmentName, RotationArray, o_rbOccluded ); - - if( Result::Success == _Result && !o_rbOccluded ) - { - MatrixToQuaternion( RotationArray, o_rFourVector ); - } - - return _Result; -} - -Result::Enum VClient::GetSegmentGlobalRotationEulerXYZ( const std::string & i_rSubjectName, - const std::string & i_rSegmentName, - double ( & o_rThreeVector)[3], - bool & o_rbOccluded ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Clear( o_rThreeVector ); - Clear( o_rbOccluded ); - // InitGet is called by GetSegmentGlobalRotationMatrix. - - // Get the answer as a rotation matrix - double RotationArray[ 9 ]; - const Result::Enum _Result = GetSegmentGlobalRotationMatrix( i_rSubjectName, i_rSegmentName, RotationArray, o_rbOccluded ); - - if( Result::Success == _Result && !o_rbOccluded ) - { - MatrixToEulerXYZ( RotationArray, o_rThreeVector ); - } - - return _Result; -} - -Result::Enum VClient::GetSegmentStaticRotationHelical( const std::string & i_rSubjectName, - const std::string & i_rSegmentName, - double ( & o_rThreeVector)[3] ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Clear( o_rThreeVector ); - - // Get the answer as a rotation matrix - double RotationArray[ 9 ]; - const Result::Enum _Result = GetSegmentStaticRotationMatrix( i_rSubjectName, i_rSegmentName, RotationArray ); - - if( Result::Success == _Result ) - { - MatrixToHelical( RotationArray, o_rThreeVector ); - } - - return _Result; -} - -Result::Enum VClient::GetSegmentStaticRotationMatrix( const std::string & i_rSubjectName, - const std::string & i_rSegmentName, - double (& o_rRotation)[9] ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rRotation ) ) - { - return GetResult; - } - - // Look up the ids for the subject and segment - unsigned int SubjectID = 0; - unsigned int SegmentID = 0; - Result::Enum _Result = GetSubjectAndSegmentID( i_rSubjectName, i_rSegmentName, SubjectID, SegmentID ); - if( Result::Success != _Result ) - { - return _Result; - } - - const ViconCGStream::VSubjectTopology * pSubjectTopology = GetSubjectTopology( SubjectID ); - if( !pSubjectTopology ) - { - return Result::InvalidSubjectName; - } - - // go through the frame's segment data and find its position in this frame - std::vector< ViconCGStreamDetail::VSubjectTopology_Segment >::const_iterator SegmentIt = pSubjectTopology->m_Segments.begin(); - std::vector< ViconCGStreamDetail::VSubjectTopology_Segment >::const_iterator SegmentEnd = pSubjectTopology->m_Segments.end(); - for( ; SegmentIt != SegmentEnd ; ++SegmentIt ) - { - const ViconCGStreamDetail::VSubjectTopology_Segment & rSegment( *SegmentIt ); - if( rSegment.m_SegmentID == SegmentID ) - { - // copy out the answer - CopyAndTransformR( rSegment.m_Rotation, o_rRotation ); - return Result::Success; - } - } - - return Result::Unknown; -} - -Result::Enum VClient::GetSegmentStaticRotationQuaternion( const std::string & i_rSubjectName, - const std::string & i_rSegmentName, - double ( & o_rFourVector)[4] ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Clear( o_rFourVector ); - - // Get the answer as a rotation matrix - double RotationArray[ 9 ]; - const Result::Enum _Result = GetSegmentStaticRotationMatrix( i_rSubjectName, i_rSegmentName, RotationArray ); - - if( Result::Success == _Result ) - { - MatrixToQuaternion( RotationArray, o_rFourVector ); - } - - return _Result; -} - -Result::Enum VClient::GetSegmentStaticRotationEulerXYZ( const std::string & i_rSubjectName, - const std::string & i_rSegmentName, - double ( & o_rThreeVector)[3] ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Clear( o_rThreeVector ); - - // Get the answer as a rotation matrix - double RotationArray[ 9 ]; - const Result::Enum _Result = GetSegmentStaticRotationMatrix( i_rSubjectName, i_rSegmentName, RotationArray ); - - if( Result::Success == _Result ) - { - MatrixToEulerXYZ( RotationArray, o_rThreeVector ); - } - - return _Result; -} - -Result::Enum VClient::GetSegmentLocalTranslation( const std::string & i_rSubjectName, - const std::string & i_rSegmentName, - double ( & o_rThreeVector)[3], - bool & o_rbOccluded ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rThreeVector, o_rbOccluded ) ) - { - return GetResult; - } - - // Look up the ids for the subject and segment - unsigned int SubjectID = 0; - unsigned int SegmentID = 0; - Result::Enum _Result = GetSubjectAndSegmentID( i_rSubjectName, i_rSegmentName, SubjectID, SegmentID ); - if( Result::Success != _Result ) - { - return _Result; - } - - // go through the frame's segment data and find its position in this frame - for (unsigned int i = 0; i < m_LatestFrame.m_LocalSegments.size(); i++) - { - const ViconCGStream::VLocalSegments& rSegments = m_LatestFrame.m_LocalSegments[i]; - if (rSegments.m_SubjectID == SubjectID) - { - // now look through its segments - for (unsigned int j = 0; j < rSegments.m_Segments.size(); j++) - { - const ViconCGStreamDetail::VLocalSegments_Segment& rSegment = rSegments.m_Segments[j]; - if (rSegment.m_SegmentID == SegmentID) - { - CopyAndTransformT( rSegment.m_Translation, o_rThreeVector ); - return Result::Success; - } - } - } - } - - // if we're here then they entered correct subject and marker info, - // but the data was not in the frame. This is okay but we must - // set the occluded flag and return true - o_rbOccluded = true; - return Result::Success; -} - -Result::Enum VClient::GetSegmentLocalRotationHelical( const std::string & i_rSubjectName, - const std::string & i_rSegmentName, - double ( & o_rThreeVector)[3], - bool & o_rbOccluded ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Clear( o_rThreeVector ); - Clear( o_rbOccluded ); - - // Get the answer as a rotation matrix - double RotationArray[ 9 ]; - const Result::Enum _Result = GetSegmentLocalRotationMatrix( i_rSubjectName, i_rSegmentName, RotationArray, o_rbOccluded ); - - if( Result::Success == _Result && !o_rbOccluded ) - { - MatrixToHelical( RotationArray, o_rThreeVector ); - } - - return _Result; -} - -Result::Enum VClient::GetSegmentLocalRotationMatrix( const std::string & i_rSubjectName, - const std::string & i_rSegmentName, - double (& o_rRotation)[9], - bool & o_rbOccluded ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rRotation, o_rbOccluded ) ) - { - return GetResult; - } - - // Look up the ids for the subject and segment - unsigned int SubjectID = 0; - unsigned int SegmentID = 0; - Result::Enum _Result = GetSubjectAndSegmentID( i_rSubjectName, i_rSegmentName, SubjectID, SegmentID ); - if( Result::Success != _Result ) - { - return _Result; - } - - // go through the frame's segment data and find its position in this frame - std::vector< ViconCGStream::VLocalSegments >::const_iterator SegmentIt = m_LatestFrame.m_LocalSegments.begin(); - std::vector< ViconCGStream::VLocalSegments >::const_iterator SegmentEnd = m_LatestFrame.m_LocalSegments.end(); - for( ; SegmentIt != SegmentEnd ; ++SegmentIt ) - { - const ViconCGStream::VLocalSegments & rSegments( *SegmentIt ); - if( rSegments.m_SubjectID == SubjectID ) - { - // now look through the segment detail - std::vector< ViconCGStreamDetail::VLocalSegments_Segment >::const_iterator SegmentDetailIt = rSegments.m_Segments.begin(); - std::vector< ViconCGStreamDetail::VLocalSegments_Segment >::const_iterator SegmentDetailEnd = rSegments.m_Segments.end(); - for( ; SegmentDetailIt != SegmentDetailEnd ; ++SegmentDetailIt ) - { - const ViconCGStreamDetail::VLocalSegments_Segment & rSegment( *SegmentDetailIt ); - if( rSegment.m_SegmentID == SegmentID ) - { - // copy out the answer - CopyAndTransformR( rSegment.m_Rotation, o_rRotation ); - o_rbOccluded = false; - return Result::Success; - } - } - } - } - - // if we're here then they entered correct subject and marker info, - // but the data was not in the frame. This is okay but we must - // set the occluded flag and return true - o_rbOccluded = true; - return Result::Success; -} - -Result::Enum VClient::GetSegmentLocalRotationQuaternion( const std::string & i_rSubjectName, - const std::string & i_rSegmentName, - double ( & o_rFourVector)[4], - bool & o_rbOccluded ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Clear( o_rFourVector ); - Clear( o_rbOccluded ); - - // Get the answer as a rotation matrix - double RotationArray[ 9 ]; - const Result::Enum _Result = GetSegmentLocalRotationMatrix( i_rSubjectName, i_rSegmentName, RotationArray, o_rbOccluded ); - - if( Result::Success == _Result && !o_rbOccluded ) - { - MatrixToQuaternion( RotationArray, o_rFourVector ); - } - - return _Result; -} - -Result::Enum VClient::GetSegmentLocalRotationEulerXYZ( const std::string & i_rSubjectName, - const std::string & i_rSegmentName, - double ( & o_rThreeVector)[3], - bool & o_rbOccluded ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Clear( o_rThreeVector ); - Clear( o_rbOccluded ); - - // Get the answer as a rotation matrix - double RotationArray[ 9 ]; - const Result::Enum _Result = GetSegmentLocalRotationMatrix( i_rSubjectName, i_rSegmentName, RotationArray, o_rbOccluded ); - - if( Result::Success == _Result && !o_rbOccluded ) - { - MatrixToEulerXYZ( RotationArray, o_rThreeVector ); - } - - return _Result; -} - -Result::Enum VClient::GetMarkerID( const ViconCGStream::VSubjectInfo & i_rSubjectInfo, const std::string& i_rMarkerName, unsigned int& o_rMarkerID ) const -{ - if( i_rMarkerName.empty() ) - { - return Result::InvalidMarkerName; - } - - const std::vector< ViconCGStreamDetail::VSubjectInfo_Marker > & rMarkers = i_rSubjectInfo.m_Markers; - for( unsigned int j = 0 ; j < rMarkers.size() ; ++j ) - { - const ViconCGStreamDetail::VSubjectInfo_Marker & rMarker = rMarkers[j]; - if( i_rMarkerName == rMarker.m_Name ) - { - o_rMarkerID = rMarker.m_MarkerID; - return Result::Success; - } - } - - return Result::InvalidMarkerName; -} - -Result::Enum VClient::GetSubjectAndMarkerID( const std::string & i_rSubjectName, - const std::string & i_rMarkerName, - unsigned int & o_rSubjectID, - unsigned int & o_rMarkerID ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Clear( o_rSubjectID ); - Clear( o_rMarkerID ); - - Result::Enum GetResult = Result::Success; - const ViconCGStream::VSubjectInfo * pSubjectInfo = GetSubjectInfo( i_rSubjectName, GetResult ); - if ( pSubjectInfo ) - { - GetResult = GetMarkerID( *pSubjectInfo, i_rMarkerName, o_rMarkerID ); - if ( GetResult == Result::Success ) - { - o_rSubjectID = pSubjectInfo->m_SubjectID; - } - } - return GetResult; -} - -Result::Enum VClient::GetSegmentID( const ViconCGStream::VSubjectInfo & i_rSubjectInfo, const std::string& i_rSegmentName, unsigned int& o_rSegmentID ) const -{ - if( i_rSegmentName.empty() ) - { - return Result::InvalidSegmentName; - } - - const std::vector< ViconCGStreamDetail::VSubjectInfo_Segment > & rSegments = i_rSubjectInfo.m_Segments; - for ( unsigned int j = 0; j < rSegments.size(); ++j ) - { - const ViconCGStreamDetail::VSubjectInfo_Segment & rSegment = rSegments[j]; - if (i_rSegmentName == rSegment.m_Name) - { - o_rSegmentID = rSegment.m_SegmentID; - return Result::Success; - } - } - return Result::InvalidSegmentName; -} - -Result::Enum VClient::GetSubjectAndSegmentID( const std::string & i_rSubjectName, - const std::string & i_rSegmentName, - unsigned int & o_rSubjectID, - unsigned int & o_rSegmentID ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Clear( o_rSubjectID ); - Clear( o_rSegmentID ); - - Result::Enum GetResult = Result::Success; - const ViconCGStream::VSubjectInfo * pSubjectInfo = GetSubjectInfo( i_rSubjectName, GetResult ); - if ( pSubjectInfo ) - { - GetResult = GetSegmentID( *pSubjectInfo, i_rSegmentName, o_rSegmentID ); - if ( GetResult == Result::Success ) - { - o_rSubjectID = pSubjectInfo->m_SubjectID; - } - } - return GetResult; -} - -const ViconCGStream::VDeviceInfo * VClient::GetDevice( const std::string & i_rDeviceName, Result::Enum & o_rResult ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - const auto DeviceIt = - std::find_if( m_LatestFrame.m_Devices.begin(), m_LatestFrame.m_Devices.end(), - [&i_rDeviceName]( const ViconCGStream::VDeviceInfo& rDevice ) - { return i_rDeviceName == AdaptDeviceName( rDevice.m_Name, rDevice.m_DeviceID ); } - ); - - if( DeviceIt != m_LatestFrame.m_Devices.end() ) - { - o_rResult = Result::Success; - return &(*DeviceIt); - } - o_rResult = Result::InvalidDeviceName; - return nullptr; -} - -Result::Enum VClient::GetDeviceID( const std::string & i_rDeviceName, unsigned int & o_rDeviceID ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Clear( o_rDeviceID ); - - Result::Enum GetResult = Result::Success; - const ViconCGStream::VDeviceInfo * pDevice = GetDevice( i_rDeviceName, GetResult ); - if ( pDevice ) - { - o_rDeviceID = pDevice->m_DeviceID; - } - return GetResult; -} - -Result::Enum VClient::GetReconRayAssignments( const std::string& i_rSubjectName, const std::string& i_rMarkerName, std::vector< unsigned int >& o_rCameraIDs, std::vector< unsigned int >& o_rCentroidIndex ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if( !InitGet( GetResult, o_rCameraIDs ) ) - { - return GetResult; - } - - unsigned int SubjectID = 0; - unsigned int MarkerID = 0; - Result::Enum _Result = GetSubjectAndMarkerID( i_rSubjectName, i_rMarkerName, SubjectID, MarkerID ); - if( Result::Success != _Result ) - { - return _Result; - } - - // go through the frame's reconstructions and find the ray contributions - for( unsigned int i = 0; i < m_LatestFrame.m_LabeledReconRayAssignments.m_ReconRayAssignments.size(); ++i ) - { - const ViconCGStreamDetail::VReconRayAssignments& rReconAssignments = m_LatestFrame.m_LabeledReconRayAssignments.m_ReconRayAssignments[i]; - if( rReconAssignments.m_SubjectID == SubjectID && rReconAssignments.m_MarkerID == MarkerID ) - { - for( const auto & rReconRay : rReconAssignments.m_ReconRays ) - { - o_rCameraIDs.push_back( rReconRay.m_CameraID ); - o_rCentroidIndex.push_back( rReconRay.m_CentroidIndex ); - } - } - } - return Result::Success; -} - -const ViconCGStream::VSubjectInfo * VClient::GetSubjectInfo( const std::string i_rSubjectName, Result::Enum & o_rResult ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - if ( !InitGet( o_rResult ) ) - { - return NULL; - } - - o_rResult = Result::InvalidSubjectName; - - if( i_rSubjectName.empty() ) - { - return NULL; - } - - std::vector< ViconCGStream::VSubjectInfo >::const_iterator It = m_LatestFrame.m_Subjects.begin(); - std::vector< ViconCGStream::VSubjectInfo >::const_iterator End = m_LatestFrame.m_Subjects.end(); - for( ; It != End ; ++It ) - { - if( i_rSubjectName == It->m_Name ) - { - o_rResult = Result::Success; - return &(*It); - } - } - - return NULL; -} - -const ViconCGStream::VSubjectTopology * VClient::GetSubjectTopology( const unsigned int i_SubjectID ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - std::vector< ViconCGStream::VSubjectTopology >::const_iterator It = m_LatestFrame.m_SubjectTopologies.begin(); - std::vector< ViconCGStream::VSubjectTopology >::const_iterator End = m_LatestFrame.m_SubjectTopologies.end(); - for( ; It != End ; ++It ) - { - if( i_SubjectID == It->m_SubjectID ) - { - return &(*It); - } - } - - return 0; -} - -const ViconCGStream::VObjectQuality * VClient::GetObjectQuality( const unsigned int i_SubjectID ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - std::vector< ViconCGStream::VObjectQuality >::const_iterator It = m_LatestFrame.m_ObjectQualities.begin(); - std::vector< ViconCGStream::VObjectQuality >::const_iterator End = m_LatestFrame.m_ObjectQualities.end(); - for( ; It != End; ++It ) - { - if( i_SubjectID == It->m_SubjectID ) - { - return &( *It ); - } - } - - return 0; -} - -const ViconCGStream::VCentroids * VClient::GetCentroidSet( const unsigned int i_CameraID, Result::Enum & o_rResult ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - const auto rCentroidSetIt = std::find_if( m_LatestFrame.m_Centroids.begin(), m_LatestFrame.m_Centroids.end(), - [&i_CameraID]( const ViconCGStream::VCentroids & rSet ) - { - return rSet.m_CameraID == i_CameraID; - } ); - - if( rCentroidSetIt != m_LatestFrame.m_Centroids.end() ) - { - o_rResult = Result::Success; - return &(*rCentroidSetIt); - } - - o_rResult = Result::InvalidIndex; - return nullptr; -} - -const ViconCGStream::VCentroidWeights * VClient::GetCentroidWeightSet( const unsigned int i_CameraID, Result::Enum & o_rResult ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - const auto rCentroidWeightSetIt = std::find_if( m_LatestFrame.m_CentroidWeights.begin(), m_LatestFrame.m_CentroidWeights.end(), - [&i_CameraID]( const ViconCGStream::VCentroidWeights & rSet ) - { - return rSet.m_CameraID == i_CameraID; - } ); - - if( rCentroidWeightSetIt != m_LatestFrame.m_CentroidWeights.end() ) - { - o_rResult = Result::Success; - return &( *rCentroidWeightSetIt ); - } - - o_rResult = Result::InvalidIndex; - return nullptr; -} - -const ViconCGStream::VGreyscaleBlobs * VClient::GetGreyscaleBlobs( const unsigned int i_CameraID, Result::Enum & o_rResult ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - const auto rGreyscaleBlobIt = std::find_if( m_LatestFrame.m_GreyscaleBlobs.begin(), m_LatestFrame.m_GreyscaleBlobs.end(), - [&i_CameraID]( const ViconCGStream::VGreyscaleBlobs & rSet ) - { - return rSet.m_CameraID == i_CameraID; - } ); - - if( rGreyscaleBlobIt != m_LatestFrame.m_GreyscaleBlobs.end() ) - { - o_rResult = Result::Success; - return &(*rGreyscaleBlobIt); - } - - o_rResult = Result::InvalidIndex; - return nullptr; -} - -void VClient::GetVideoFrame( const unsigned int i_CameraID, Result::Enum & o_rResult, ViconCGStreamClientSDK::VVideoFramePtr & o_rVideoFramePtr ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - const auto rVideoFramePtrIt = std::find_if( m_LatestFrame.m_VideoFrames.begin(), m_LatestFrame.m_VideoFrames.end(), - [&i_CameraID]( const ViconCGStreamClientSDK::VVideoFramePtr & rPtr ) - { - return (*rPtr).m_CameraID == i_CameraID; - } ); - - if( rVideoFramePtrIt != m_LatestFrame.m_VideoFrames.end() ) - { - o_rResult = Result::Success; - o_rVideoFramePtr = *rVideoFramePtrIt; - } - else - { - o_rResult = Result::InvalidIndex; - o_rVideoFramePtr.Clear(); - } -} - -bool VClient::IsForcePlateDevice( unsigned int i_DeviceID ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - unsigned int RelevantChannels = 0; - - // check for any channel information that would mean this is a forceplate - for (unsigned int j = 0; j < m_LatestFrame.m_Channels.size(); j++) - { - const ViconCGStream::VChannelInfo& rChannel = m_LatestFrame.m_Channels[j]; - - if (i_DeviceID == rChannel.m_DeviceID && IsForcePlateCoreChannel(rChannel)) - { - RelevantChannels++; - } - } - - return 3 == RelevantChannels; -} - -// the function below determines whether the device channel is one of the "core" forceplate -// data channels that are always present, namely Forces, Moments and Centre of Pressure. -// There might be other channels for forceplates, but they would always have one of these. -bool VClient::IsForcePlateCoreChannel( const ViconCGStream::VChannelInfo & rChannel ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - return IsForcePlateForceChannel( rChannel ) || - IsForcePlateMomentChannel( rChannel ) || - IsForcePlateCoPChannel( rChannel ); -} - -bool VClient::IsForcePlateForceChannel( const ViconCGStream::VChannelInfo & rChannel ) const -{ - const std::vector< std::string > & rNames = rChannel.m_ComponentNames; - - return ( rNames.size() == 3 ) && - ( rNames[ 0 ] == "Fx" ) && - ( rNames[ 1 ] == "Fy" ) && - ( rNames[ 2 ] == "Fz" ); -} - -bool VClient::IsForcePlateMomentChannel( const ViconCGStream::VChannelInfo & rChannel ) const -{ - const std::vector< std::string > & rNames = rChannel.m_ComponentNames; - - return ( rNames.size() == 3 ) && - ( rNames[ 0 ] == "Mx" ) && - ( rNames[ 1 ] == "My" ) && - ( rNames[ 2 ] == "Mz" ); -} - -bool VClient::IsForcePlateCoPChannel( const ViconCGStream::VChannelInfo & rChannel ) const -{ - const std::vector< std::string > & rNames = rChannel.m_ComponentNames; - - return ( rNames.size() == 3 ) && - ( rNames[ 0 ] == "Cx" ) && - ( rNames[ 1 ] == "Cy" ) && - ( rNames[ 2 ] == "Cz" ); -} - -Result::Enum VClient::GetForcePlateCount( unsigned int & o_rCount ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rCount ) ) - { - return GetResult; - } - - unsigned int ForcePlates = 0; - - for( unsigned int i = 0; i < m_LatestFrame.m_Devices.size(); i++ ) - { - if( IsForcePlateDevice( m_LatestFrame.m_Devices[i].m_DeviceID ) ) - { - ForcePlates++; - } - } - - o_rCount = ForcePlates; - return Result::Success; -} - -Result::Enum VClient::GetForcePlateID( unsigned int i_ZeroIndexedPlateIndex, unsigned int& o_rPlateID ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rPlateID ) ) - { - return GetResult; - } - - unsigned int ForcePlates = 0; - - for( unsigned int i = 0 ; i < m_LatestFrame.m_Devices.size() ; ++i ) - { - if( !IsForcePlateDevice( m_LatestFrame.m_Devices[i].m_DeviceID ) ) - { - continue; - } - - if( ForcePlates == i_ZeroIndexedPlateIndex ) - { - // this is our forceplate - o_rPlateID = m_LatestFrame.m_Devices[i].m_DeviceID; - return Result::Success; - } - else - { - ++ForcePlates; - } - } - - return Result::InvalidIndex; -} - -bool VClient::ForcePlateDeviceIndex( const unsigned int i_DeviceID, unsigned int & o_rZeroBasedIndex ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - for(unsigned int j = 0; j < m_LatestFrame.m_ForcePlates.size(); ++j ) - { - const ViconCGStream::VForcePlateInfo & rForcePlate = m_LatestFrame.m_ForcePlates[j]; - - if( i_DeviceID == rForcePlate.m_DeviceID ) - { - o_rZeroBasedIndex = j; - return true; - } - } - - return false; -} - -// Internal access for the subsamples. -Result::Enum VClient::ForcePlateSubsamples( unsigned int i_PlateID, unsigned int & o_rForcePlateSubsamples ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rForcePlateSubsamples ) ) - { - return GetResult; - } - - if( !IsForcePlateDevice( i_PlateID ) ) - { - return Result::InvalidIndex; - } - - const ViconCGStreamType::UInt64 DevicePeriod = GetDevicePeriod( i_PlateID ); - const ViconCGStreamType::UInt64 DeviceStartTick = GetDeviceStartTick( i_PlateID ); - const TPeriod FramePeriod = GetFramePeriod( m_LatestFrame ); - - for( unsigned int i = 0 ; i < m_LatestFrame.m_Forces.size() ; ++i ) - { - const ViconCGStream::VForceFrame& rForces = m_LatestFrame.m_Forces[i]; - if( rForces.m_DeviceID == i_PlateID ) - { - const size_t NumSamples = rForces.m_Samples.size() / 3; - if ( !NumSamples || DevicePeriod % NumSamples != 0 ) - { - return Result::Unknown; - } - - const ViconCGStreamType::UInt64 SamplePeriod = DevicePeriod / NumSamples; - - o_rForcePlateSubsamples = GetSamplesInFrame( FramePeriod, DeviceStartTick, SamplePeriod ); - return Result::Success; - } - } - - return Result::Unknown; -} - -template < typename T > -Result::Enum ViconDataStreamSDK::Core::VClient::GetForcePlateVector( const unsigned int i_PlateID, - const unsigned int i_ForcePlateSubsamples, - const std::vector< T > & i_rFrameVector, - boost::array< double, 3 > & o_rForcePlateVector ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult ) ) - { - return GetResult; - } - - if( !IsForcePlateDevice( i_PlateID ) ) - { - return Result::InvalidIndex; - } - - const ViconCGStreamType::UInt64 DevicePeriod = GetDevicePeriod( i_PlateID ); - - if ( !DevicePeriod ) - { - return Result::InvalidIndex; - } - - const ViconCGStreamType::UInt64 DeviceOffset = GetDeviceStartTick( i_PlateID ); - - const TPeriod FramePeriod = GetFramePeriod( m_LatestFrame ); - - for( unsigned int i = 0 ; i < i_rFrameVector.size() ; ++i ) - { - const T& rFrame = i_rFrameVector[i]; - if( rFrame.m_DeviceID == i_PlateID ) - { - const size_t NumSamples = rFrame.m_Samples.size() / 3; - - if( NumSamples == 0 || DevicePeriod % NumSamples != 0 ) - { - return Result::Unknown; - } - - const ViconCGStreamType::UInt64 SamplePeriod = DevicePeriod / NumSamples; - - if ( i_ForcePlateSubsamples >= GetSamplesInFrame( FramePeriod, DeviceOffset, SamplePeriod ) ) - { - // Out of range for the entire frame. - return Result::InvalidIndex; - } - - unsigned int SubSample = 0; - if( !SampleIndexInRange( i_ForcePlateSubsamples, rFrame.m_FrameID, NumSamples, SamplePeriod, DeviceOffset, FramePeriod, SubSample ) ) - { - // Not in the current set of samples - might be in a later one though. - continue; - } - - o_rForcePlateVector[0] = rFrame.m_Samples[ 0 + SubSample ]; - o_rForcePlateVector[1] = rFrame.m_Samples[ NumSamples + SubSample ]; - o_rForcePlateVector[2] = rFrame.m_Samples[ NumSamples * 2 + SubSample ]; - - return Result::Success; - } - } - - return Result::Unknown; -} - -// Internal function used by local and global force functions. -Result::Enum VClient::GetForceVector( const unsigned int i_PlateID, - const unsigned int i_ForcePlateSubsamples, - boost::array< double, 3 > & o_rForceVector ) const -{ - return GetForcePlateVector( i_PlateID, i_ForcePlateSubsamples, m_LatestFrame.m_Forces, o_rForceVector ); -} - -// Internal function used by local and global moment functions. -Result::Enum VClient::GetMomentVector( const unsigned int i_PlateID, - const unsigned int i_ForcePlateSubsamples, - boost::array< double, 3 > & o_rMomentVector ) const -{ - return GetForcePlateVector( i_PlateID, i_ForcePlateSubsamples, m_LatestFrame.m_Moments, o_rMomentVector ); -} - -// Internal function used by local and global CoP functions. -Result::Enum VClient::GetCentreOfPressure( const unsigned int i_PlateID, - const unsigned int i_ForcePlateSubsamples, - boost::array< double, 3 > & o_rLocation ) const -{ - return GetForcePlateVector( i_PlateID, i_ForcePlateSubsamples, m_LatestFrame.m_CentresOfPressure, o_rLocation ); -} - -Result::Enum VClient::GetForceVector( const unsigned int i_PlateID, - const unsigned int i_Subsample, - double ( & o_rForceVector)[3] ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Clear( o_rForceVector ); - - boost::array< double, 3 > ForceVector; - Result::Enum Result = GetForceVector( i_PlateID, i_Subsample, ForceVector ); - - if( Result == Result::Success ) - { - CopyAndTransformT( ForceVector.data(), o_rForceVector ); - } - - return Result; -} - -Result::Enum VClient::GetMomentVector( const unsigned int i_PlateID, - const unsigned int i_Subsample, - double ( & o_rMomentVector )[3] ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Clear( o_rMomentVector ); - - boost::array< double, 3 > MomentVector; - Result::Enum Result = GetMomentVector( i_PlateID, i_Subsample, MomentVector ); - - if( Result == Result::Success ) - { - CopyAndTransformT( MomentVector.data(), o_rMomentVector ); - } - - return Result; -} - -Result::Enum VClient::GetCentreOfPressure( const unsigned int i_PlateID, - const unsigned int i_Subsample, - double ( & o_rLocation )[3] ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Clear( o_rLocation ); - - boost::array< double, 3 > Location; - Result::Enum Result = GetCentreOfPressure( i_PlateID, i_Subsample, Location ); - - if( Result == Result::Success ) - { - CopyAndTransformT( Location.data(), o_rLocation ); - } - - return Result; -} - -Result::Enum VClient::GetGlobalForceVector( const unsigned int i_PlateID, - const unsigned int i_Subsample, - double ( & o_rForceVector)[3] ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Clear( o_rForceVector ); - - boost::array< double, 3 > ForceVector; - - Result::Enum Result = GetForceVector( i_PlateID, i_Subsample, ForceVector ); - - if( Result == Result::Success ) - { - // Get the plate index. - - unsigned int PlateIndex; - if( !ForcePlateDeviceIndex( i_PlateID, PlateIndex ) ) - { - return Result::Unknown; - } - - // Transform result to global coordinates by rotating by plate orientation. - - const ViconCGStream::VForcePlateInfo & rForcePlate = m_LatestFrame.m_ForcePlates[ PlateIndex ]; - - boost::array< double, 3 * 3 > WorldRotation; - std::copy( rForcePlate.m_WorldRotation, rForcePlate.m_WorldRotation + 9, WorldRotation.begin() ); - - boost::array< double, 3 > WorldForceVector; - - WorldForceVector = WorldRotation * ForceVector; - - CopyAndTransformT( WorldForceVector.data(), o_rForceVector ); - } - - return Result; -} - -Result::Enum VClient::GetGlobalMomentVector( const unsigned int i_PlateID, - const unsigned int i_Subsample, - double ( & o_rMomentVector )[3] ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Clear( o_rMomentVector ); - - boost::array< double, 3 > MomentVector; - Result::Enum Result = GetMomentVector( i_PlateID, i_Subsample, MomentVector ); - - if( Result == Result::Success ) - { - unsigned int PlateIndex; - if( !ForcePlateDeviceIndex( i_PlateID, PlateIndex ) ) - { - return Result::Unknown; - } - - const ViconCGStream::VForcePlateInfo & rForcePlate = m_LatestFrame.m_ForcePlates[ PlateIndex ]; - - boost::array< double, 3 * 3 > WorldRotation; - std::copy( rForcePlate.m_WorldRotation, rForcePlate.m_WorldRotation + 9, WorldRotation.begin() ); - - boost::array< double, 3 > WorldMomentVector; - - WorldMomentVector = WorldRotation * MomentVector; - - CopyAndTransformT( WorldMomentVector.data(), o_rMomentVector ); - } - - return Result; -} - -Result::Enum VClient::GetGlobalCentreOfPressure( const unsigned int i_PlateID, - const unsigned int i_Subsample, - double ( & o_rLocation )[3] ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Clear( o_rLocation ); - - boost::array< double, 3 > LocationVector; - Result::Enum Result = GetCentreOfPressure( i_PlateID, i_Subsample, LocationVector ); - - if( Result == Result::Success ) - { - unsigned int PlateIndex; - if( !ForcePlateDeviceIndex( i_PlateID, PlateIndex ) ) - { - return Result::Unknown; - } - - const ViconCGStream::VForcePlateInfo & rForcePlate = m_LatestFrame.m_ForcePlates[ PlateIndex ]; - - boost::array< double, 3 * 3 > WorldRotation; - std::copy( rForcePlate.m_WorldRotation, rForcePlate.m_WorldRotation + 9, WorldRotation.begin() ); - - boost::array< double, 3 > WorldTranslation; - std::copy( rForcePlate.m_WorldTranslation, rForcePlate.m_WorldTranslation + 3, WorldTranslation.begin() ); - - // Force Plate Info is in mm. Result CoP is in meters. - WorldTranslation /= 1000.0; - - boost::array< double, 3 > WorldLocationVector; - - WorldLocationVector = WorldRotation * LocationVector + WorldTranslation; - - CopyAndTransformT( WorldLocationVector.data(), o_rLocation ); - } - - return Result; -} - -Result::Enum VClient::GetForcePlateSubsamples( const unsigned int i_PlateID, unsigned int& o_rForcePlateSubsamples ) const -{ - return ForcePlateSubsamples( i_PlateID, o_rForcePlateSubsamples ); -} - -Result::Enum VClient::GetForceVector( const unsigned int i_PlateID, double ( & o_rForceVector)[3] ) const -{ - return GetForceVector( i_PlateID, 0, o_rForceVector ); -} - -Result::Enum VClient::GetMomentVector( const unsigned int i_PlateID, double ( & o_rMomentVector)[3] ) const -{ - return GetMomentVector( i_PlateID, 0, o_rMomentVector ); -} - -Result::Enum VClient::GetCentreOfPressure( const unsigned int i_PlateID, double ( & o_rLocation )[3] ) const -{ - return GetCentreOfPressure( i_PlateID, 0, o_rLocation ); -} - -Result::Enum VClient::GetGlobalForceVector( const unsigned int i_PlateID, double ( & o_rForceVector)[3] ) const -{ - return GetGlobalForceVector( i_PlateID, 0, o_rForceVector ); -} - -Result::Enum VClient::GetGlobalMomentVector( const unsigned int i_PlateID, double ( & o_rMomentVector)[3] ) const -{ - return GetGlobalMomentVector( i_PlateID, 0, o_rMomentVector ); -} - -Result::Enum VClient::GetGlobalCentreOfPressure( const unsigned int i_PlateID, double ( & o_rLocation )[3] ) const -{ - return GetGlobalCentreOfPressure( i_PlateID, 0, o_rLocation ); -} - -// Comment explaining about analog components -Result::Enum VClient::GetNumberOfAnalogChannels( const unsigned int i_PlateID, unsigned int& o_rChannelCount ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rChannelCount ) ) - { - return GetResult; - } - - if( !IsForcePlateDevice( i_PlateID ) ) - { - return Result::InvalidIndex; - } - - // now find channel information that is not - for( size_t j = 0 ; j < m_LatestFrame.m_Channels.size() ; ++j ) - { - const ViconCGStream::VChannelInfo& rChannel = m_LatestFrame.m_Channels[j]; - - if( i_PlateID == rChannel.m_DeviceID && !IsForcePlateCoreChannel( rChannel ) ) - { - // we assume it is an analogue channel - o_rChannelCount = static_cast< unsigned int >( rChannel.m_ComponentNames.size() ); - return Result::Success; - } - } - return Result::Unknown; -} - -Result::Enum VClient::GetAnalogChannelVoltage( const unsigned int i_PlateID, - const unsigned int i_ChannelIndex, - double & o_rVoltage) const -{ - return GetAnalogChannelVoltage( i_PlateID, i_ChannelIndex, 0, o_rVoltage ); -} - -Result::Enum VClient::GetAnalogChannelVoltage( const unsigned int i_PlateID, - const unsigned int i_ChannelIndex, - const unsigned int i_Subsample, - double & o_rVoltage) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rVoltage ) ) - { - return GetResult; - } - - if( !IsForcePlateDevice( i_PlateID ) ) - { - return Result::InvalidIndex; - } - - const ViconCGStreamType::UInt64 DevicePeriod = GetDevicePeriod( i_PlateID ); - - if ( !DevicePeriod ) - { - return Result::InvalidIndex; - } - - unsigned int ChannelID = 0; - unsigned int VoltageComponentsPerSample = 0; - bool bFoundChannelID = false; - - // get the channel ID for the voltage channel of this plate - for( size_t j = 0 ; j < m_LatestFrame.m_Channels.size() ; ++j ) - { - const ViconCGStream::VChannelInfo& rChannel = m_LatestFrame.m_Channels[j]; - - if( i_PlateID == rChannel.m_DeviceID && !IsForcePlateCoreChannel( rChannel ) ) - { - ChannelID = rChannel.m_ChannelID; - VoltageComponentsPerSample = static_cast< unsigned int >( rChannel.m_ComponentNames.size() ); - bFoundChannelID = true; - break; - } - } - - if( !bFoundChannelID ) - { - return Result::InvalidIndex; - } - - const TPeriod FramePeriod = GetFramePeriod( m_LatestFrame ); - const ViconCGStreamType::UInt64 DeviceStartTick = GetDeviceStartTick( i_PlateID ); - - // now look through the voltage channels for this ID - // subfactor the voltage values by "VoltageComponentsPerSample" - - for( size_t i = 0 ; i < m_LatestFrame.m_Voltages.size() ; ++i ) - { - const ViconCGStream::VVoltageFrame& rVoltages = m_LatestFrame.m_Voltages[i]; - - if( rVoltages.m_ChannelID == ChannelID ) - { - // get the voltages out of here - size_t NumberOfVoltageSets = rVoltages.m_Samples.size() / VoltageComponentsPerSample; - - if( 0 == NumberOfVoltageSets || DevicePeriod % NumberOfVoltageSets != 0 ) - { - return Result::Unknown; - } - - const ViconCGStreamType::UInt64 SamplePeriod = DevicePeriod / NumberOfVoltageSets; - - if ( i_Subsample >= GetSamplesInFrame( FramePeriod, DeviceStartTick, SamplePeriod ) ) - { - // Out of range for the entire frame. - return Result::InvalidIndex; - } - - unsigned int SubSample = 0; - if( !SampleIndexInRange( i_Subsample, rVoltages.m_FrameID, NumberOfVoltageSets, SamplePeriod, DeviceStartTick, FramePeriod, SubSample ) ) - { - // Not in the current set of samples - might be in a later one though. - continue; - } - - // Look up the sample we want. - o_rVoltage = rVoltages.m_Samples[ i_ChannelIndex * NumberOfVoltageSets + SubSample ]; - return Result::Success; - } - } - - return Result::Unknown; - -} - -Result::Enum VClient::GetEyeTrackerCount( unsigned int & o_rCount ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( InitGet( GetResult, o_rCount ) ) - { - o_rCount = static_cast( m_LatestFrame.m_EyeTrackers.size() ); - } - return GetResult; -} - -Result::Enum VClient::GetEyeTrackerID( const unsigned int i_EyeTrackerIndex, unsigned int& o_rEyeTrackerID ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rEyeTrackerID ) ) - { - return GetResult; - } - - if( i_EyeTrackerIndex < m_LatestFrame.m_EyeTrackers.size() ) - { - o_rEyeTrackerID = m_LatestFrame.m_EyeTrackers[ i_EyeTrackerIndex ].m_DeviceID; - return Result::Success; - } - - return Result::InvalidIndex; -} - -Result::Enum VClient::GetEyeTrackerGlobalPosition( const unsigned int i_EyeTrackerID, double (&o_rThreeVector)[3], bool& o_rbOccludedFlag ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rThreeVector, o_rbOccludedFlag ) ) - { - return GetResult; - } - - size_t EyeTrackerIndex = -1; - - for( size_t i = 0; i < m_LatestFrame.m_EyeTrackers.size(); i++ ) - { - if( m_LatestFrame.m_EyeTrackers[ i ].m_DeviceID == i_EyeTrackerID ) - { - EyeTrackerIndex = i; - } - } - - if( EyeTrackerIndex == BadIndex ) - { - return Result::InvalidIndex; - } - - const ViconCGStream::VEyeTrackerInfo & rEyeTracker = m_LatestFrame.m_EyeTrackers[ EyeTrackerIndex ]; - - // Look up the ids for the subject and segment - unsigned int SubjectID = rEyeTracker.m_SubjectID; - unsigned int SegmentID = rEyeTracker.m_SegmentID; - - // go through the frame's segment data and find its position in this frame - for( unsigned int i = 0; i < m_LatestFrame.m_GlobalSegments.size(); i++ ) - { - const ViconCGStream::VGlobalSegments& rSegments = m_LatestFrame.m_GlobalSegments[i]; - if( rSegments.m_SubjectID == SubjectID ) - { - // now look through its segments - for( unsigned int j = 0; j < rSegments.m_Segments.size(); j++ ) - { - const ViconCGStreamDetail::VGlobalSegments_Segment& rSegment = rSegments.m_Segments[j]; - if( rSegment.m_SegmentID == SegmentID ) - { - // Use the segment to calculate global eye location. - - boost::array< double, 3 * 3 > WorldRotation; - std::copy( rSegment.m_Rotation, rSegment.m_Rotation + 9, WorldRotation.begin() ); - - boost::array< double, 3 > WorldTranslation; - std::copy( rSegment.m_Translation, rSegment.m_Translation + 3, WorldTranslation.begin() ); - - boost::array< double, 3 > EyeTranslation; - std::copy( rEyeTracker.m_LocalTranslation, rEyeTracker.m_LocalTranslation + 3, EyeTranslation.begin() ); - - const boost::array< double, 3 > WorldEyeTranslation = WorldRotation * EyeTranslation + WorldTranslation; - - CopyAndTransformT( WorldEyeTranslation.data(), o_rThreeVector ); - return Result::Success; - } - } - } - } - - // If we fail to find the segment it is probably just failed to fit. - // We mark this as an occluded success. - - o_rbOccludedFlag = true; - return Result::Success; -} - -Result::Enum VClient::GetEyeTrackerGlobalGazeVector( const unsigned int i_EyeTrackerID, double (&o_rThreeVector)[3], bool& o_rbOccludedFlag ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rThreeVector, o_rbOccludedFlag ) ) - { - return GetResult; - } - - size_t EyeTrackerIndex = -1; - - for( size_t i = 0; i < m_LatestFrame.m_EyeTrackers.size(); i++ ) - { - if( m_LatestFrame.m_EyeTrackers[ i ].m_DeviceID == i_EyeTrackerID ) - { - EyeTrackerIndex = i; - } - } - - if( EyeTrackerIndex == BadIndex ) - { - return Result::InvalidIndex; - } - - const ViconCGStream::VEyeTrackerInfo & rEyeTracker = m_LatestFrame.m_EyeTrackers[ EyeTrackerIndex ]; - - size_t EyeTrackIndex = -1; - - for( size_t i = 0; i < m_LatestFrame.m_EyeTracks.size(); i++ ) - { - if( m_LatestFrame.m_EyeTracks[ i ].m_DeviceID == i_EyeTrackerID ) - { - EyeTrackIndex = i; - } - } - - if( EyeTrackIndex == BadIndex ) - { - // If we fail to find the eye track information then the - // the user is probably blinking or similar. - // This is a successfull call for an occluded sample. - o_rbOccludedFlag = true; - return Result::Success; - } - - const ViconCGStream::VEyeTrackerFrame & rEyeTrack = m_LatestFrame.m_EyeTracks[ EyeTrackIndex ]; - - // Look up the ids for the subject and segment - unsigned int SubjectID = rEyeTracker.m_SubjectID; - unsigned int SegmentID = rEyeTracker.m_SegmentID; - - // go through the frame's segment data and find its position in this frame - for( unsigned int i = 0; i < m_LatestFrame.m_GlobalSegments.size(); i++ ) - { - const ViconCGStream::VGlobalSegments& rSegments = m_LatestFrame.m_GlobalSegments[i]; - if( rSegments.m_SubjectID == SubjectID ) - { - // now look through its segments - for( unsigned int j = 0; j < rSegments.m_Segments.size(); j++ ) - { - const ViconCGStreamDetail::VGlobalSegments_Segment& rSegment = rSegments.m_Segments[j]; - if( rSegment.m_SegmentID == SegmentID ) - { - // Use the segment to calculate global eye location. - - boost::array< double, 3 * 3 > WorldRotation; - std::copy( rSegment.m_Rotation, rSegment.m_Rotation + 9, WorldRotation.begin() ); - - boost::array< double, 3 * 3 > EyeRotation; - std::copy( rEyeTracker.m_LocalRotation, rEyeTracker.m_LocalRotation + 9, EyeRotation.begin() ); - - boost::array< double, 3 > EyeGaze; - std::copy( rEyeTrack.m_GazeVector, rEyeTrack.m_GazeVector + 3, EyeGaze.begin() ); - - const boost::array< double, 3 > WorldGazeVector = ( WorldRotation * EyeRotation ) * EyeGaze; - - CopyAndTransformT( WorldGazeVector.data(), o_rThreeVector ); - return Result::Success; - } - } - } - } - - // No present head segment. - o_rbOccludedFlag = true; - return Result::Success; -} - -bool VClient::IsEyeTrackerDevice(unsigned int i_DeviceID) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - for( unsigned int i = 0; i < m_LatestFrame.m_EyeTrackers.size(); i++ ) - { - if( m_LatestFrame.m_EyeTrackers[ i ].m_DeviceID == i_DeviceID ) - { - return true; - } - } - - return false; -} - -bool VClient::HasData() const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - return BadFrameValue != m_LatestFrame.m_Frame.m_FrameID; -} - -void VClient::PreFetchThreadBody() -{ - while( m_bContinuePreFetch ) - { - FetchNextFrame(); - } -} - -void VClient::FetchNextFrame() -{ - if( !m_pClient ) - { - return; - } - - std::vector< ViconCGStreamClientSDK::ICGFrameState > LoadedFrames; - - ViconCGStreamClientSDK::ICGFrameState Frame; - if( m_pClient->WaitFrame( Frame, s_WaitFrameTimeout ) ) - { - // copy out the last frame - m_bNewCachedFrame = true; - m_CachedFrame = Frame; - } -} - -Result::Enum VClient::GetDeviceCount( unsigned int & o_rDeviceCount ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( InitGet( GetResult, o_rDeviceCount ) ) - { - o_rDeviceCount = static_cast< unsigned int >( m_LatestFrame.m_Devices.size() ); - } - return GetResult; -} - -Result::Enum VClient::GetDeviceName( const unsigned int i_DeviceIndex, - std::string & o_rDeviceName, - DeviceType::Enum & o_rDeviceType ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rDeviceName, o_rDeviceType ) ) - { - return GetResult; - } - - if( i_DeviceIndex >= m_LatestFrame.m_Devices.size() ) - { - return Result::InvalidIndex; - } - - const ViconCGStream::VDeviceInfo & rDevice( m_LatestFrame.m_Devices[ i_DeviceIndex ] ); - o_rDeviceName = AdaptDeviceName( rDevice.m_Name, rDevice.m_DeviceID ); - if( IsForcePlateDevice( rDevice.m_DeviceID ) ) - { - o_rDeviceType = DeviceType::ForcePlate; - } - else if( IsEyeTrackerDevice( rDevice.m_DeviceID ) ) - { - o_rDeviceType = DeviceType::EyeTracker; - } - else - { - o_rDeviceType = DeviceType::Unknown; - } - - return Result::Success; -} - -Result::Enum VClient::GetDeviceOutputCount( const std::string & i_rDeviceName, - unsigned int & o_rDeviceOutputCount ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rDeviceOutputCount ) ) - { - return GetResult; - } - - // Get the device id - unsigned int DeviceID = 0; - Result::Enum _Result = GetDeviceID( i_rDeviceName, DeviceID ); - if( Result::Success != _Result ) - { - return _Result; - } - - // Iterate over the channels for this device - std::vector< ViconCGStream::VChannelInfo >::const_iterator ChannelIt = m_LatestFrame.m_Channels.begin(); - std::vector< ViconCGStream::VChannelInfo >::const_iterator ChannelEnd = m_LatestFrame.m_Channels.end(); - for( ; ChannelIt != ChannelEnd ; ++ChannelIt ) - { - const ViconCGStream::VChannelInfo & rChannel( *ChannelIt ); - - if( rChannel.m_DeviceID == DeviceID ) - { - o_rDeviceOutputCount += static_cast< unsigned int >( rChannel.m_ComponentNames.size() ); - } - } - - return Result::Success; -} - -namespace -{ - template< typename TFrame > - bool GetSamples( const std::vector< TFrame > & i_rFrames, - const ViconCGStream::VChannelInfo & i_rChannel, - const unsigned int i_Subsample, - const ViconCGStreamType::UInt64 i_DevicePeriod, - const ViconCGStreamType::UInt64 i_DeviceStartTick, - const TPeriod & i_rFramePeriod, - std::vector< double > & o_rSamples, - bool & o_rbOccluded ) - { - o_rSamples.clear(); - o_rbOccluded = true; - - typename std::vector< TFrame >::const_iterator It = i_rFrames.begin(); - typename std::vector< TFrame >::const_iterator End = i_rFrames.end(); - - const size_t NumComponents = i_rChannel.m_ComponentNames.size(); - for( ; It != End ; ++It ) - { - const TFrame & rFrame( *It ); - if( rFrame.m_DeviceID == i_rChannel.m_DeviceID && - rFrame.m_ChannelID == i_rChannel.m_ChannelID && - rFrame.m_Samples.size() >= NumComponents && - NumComponents != 0 ) - { - const size_t Stride = rFrame.m_Samples.size() / NumComponents; - - assert( i_DevicePeriod % Stride == 0 ); - - const ViconCGStreamType::UInt64 SamplePeriod = i_DevicePeriod / Stride; - - if ( i_Subsample >= GetSamplesInFrame( i_rFramePeriod, i_DeviceStartTick, SamplePeriod ) ) - { - // Out of range for the entire frame. - return false; - } - - unsigned int SubSample = 0; - if( !SampleIndexInRange( i_Subsample, rFrame.m_FrameID, Stride, SamplePeriod, i_DeviceStartTick, i_rFramePeriod, SubSample ) ) - { - // Not in the current set of samples - might be in a later one though. - continue; - } - - for( size_t i = 0 ; i < NumComponents ; ++i ) - { - o_rSamples.push_back( rFrame.m_Samples[ i * Stride + SubSample ] ); - } - - o_rbOccluded = false; - break; - } - } - - return true; - } - - template< typename TFrame, unsigned int N > - bool GetSamples( const std::vector< TFrame > & i_rFrames, - const ViconCGStream::VChannelInfo & i_rChannel, - const unsigned int i_Subsample, - const ViconCGStreamType::UInt64 i_DevicePeriod, - const ViconCGStreamType::UInt64 i_DeviceStartTick, - const TPeriod & i_rFramePeriod, - double (& o_rSamples)[ N ], - bool & o_rbOccluded ) - { - assert( i_rChannel.m_ComponentNames.size() == N ); - std::vector< double > Samples; - bool bOK = GetSamples< TFrame >( i_rFrames, i_rChannel, i_Subsample, i_DevicePeriod, i_DeviceStartTick, i_rFramePeriod, Samples, o_rbOccluded ); - - if( !o_rbOccluded ) - { - assert( Samples.size() == N ); - std::copy( Samples.begin(), Samples.begin() + N, o_rSamples ); - } - - return bOK; - } - - template< typename TFrame > - bool GetSampleCount( const std::vector< TFrame > & i_rFrames, - const ViconCGStream::VChannelInfo & i_rChannel, - const ViconCGStreamType::UInt64 i_DevicePeriod, - const ViconCGStreamType::UInt64 i_DeviceStartTick, - const TPeriod & i_rFramePeriod, - unsigned int & o_rCount ) - { - typename std::vector< TFrame >::const_iterator It = i_rFrames.begin(); - typename std::vector< TFrame >::const_iterator End = i_rFrames.end(); - o_rCount = 0; - const size_t Components = i_rChannel.m_ComponentNames.size(); - for( ; It != End ; ++It ) - { - const TFrame & rFrame( *It ); - const size_t NumberOfSampleComponents = rFrame.m_Samples.size(); - if( rFrame.m_DeviceID == i_rChannel.m_DeviceID && - rFrame.m_ChannelID == i_rChannel.m_ChannelID && - NumberOfSampleComponents >= Components && - Components != 0 ) - { - assert( NumberOfSampleComponents % Components == 0 ); - - const size_t NumSamples = NumberOfSampleComponents / Components; - - assert( i_DevicePeriod % NumSamples == 0 ); - const ViconCGStreamType::Int64 SamplePeriod = i_DevicePeriod / NumSamples; - - o_rCount = GetSamplesInFrame( i_rFramePeriod, i_DeviceStartTick, SamplePeriod ); - return true; - } - } - return false; - } -} - -Result::Enum VClient::GetDeviceOutputName( const std::string & i_rDeviceName, - const unsigned int i_DeviceOutputIndex, - std::string & o_rDeviceOutputName, - Unit::Enum & o_rDeviceOutputUnit ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rDeviceOutputName, o_rDeviceOutputUnit ) ) - { - return GetResult; - } - - // Get the device id - unsigned int DeviceID = 0; - Result::Enum _Result = GetDeviceID( i_rDeviceName, DeviceID ); - if( Result::Success != _Result ) - { - return _Result; - } - - // Is this a forceplate? - const bool bIsForcePlate( IsForcePlateDevice( DeviceID ) ); - - // Iterate over the channels for this device - unsigned int CurrentDeviceOutputIndex = 0; - std::vector< ViconCGStream::VChannelInfo >::const_iterator ChannelIt = m_LatestFrame.m_Channels.begin(); - const std::vector< ViconCGStream::VChannelInfo >::const_iterator ChannelEnd = m_LatestFrame.m_Channels.end(); - for( ; ChannelIt != ChannelEnd ; ++ChannelIt ) - { - const ViconCGStream::VChannelInfo & rChannel( *ChannelIt ); - - // Wrong device - if( rChannel.m_DeviceID != DeviceID ) - { - continue; - } - - std::vector< std::string >::const_iterator ComponentIt = rChannel.m_ComponentNames.begin(); - std::vector< std::string >::const_iterator ComponentEnd = rChannel.m_ComponentNames.end(); - for( ; ComponentIt != ComponentEnd ; ++ComponentIt ) - { - if( CurrentDeviceOutputIndex == i_DeviceOutputIndex ) - { - o_rDeviceOutputName = AdaptDeviceOutputName( *ComponentIt, CurrentDeviceOutputIndex ); - - if( bIsForcePlate ) - { - // Determine where the data lives in this channel - if( IsForcePlateForceChannel( rChannel ) ) - { - o_rDeviceOutputUnit = Unit::Newton; - } - else if( IsForcePlateMomentChannel( rChannel ) ) - { - o_rDeviceOutputUnit = Unit::NewtonMeter; - } - else if( IsForcePlateCoPChannel( rChannel ) ) - { - o_rDeviceOutputUnit = Unit::Meter; - } - else - { - o_rDeviceOutputUnit = Unit::Volt; - } - } - else - { - // Not a forceplate. - o_rDeviceOutputUnit = Unit::Unknown; - - // Look for information in the extra channel information. - - std::vector< ViconCGStream::VChannelInfoExtra >::const_iterator ChannelUnitIt = m_LatestFrame.m_ChannelUnits.begin(); - const std::vector< ViconCGStream::VChannelInfoExtra >::const_iterator ChannelUnitEnd = m_LatestFrame.m_ChannelUnits.end(); - for( ; ChannelUnitIt != ChannelUnitEnd ; ++ChannelUnitIt ) - { - const ViconCGStream::VChannelInfoExtra & rChannelInfoExtra( *ChannelUnitIt ); - - // Wrong Device - if( rChannelInfoExtra.m_DeviceID != DeviceID ) - { - continue; - } - - // Wrong Channel - if( rChannelInfoExtra.m_ChannelID != rChannel.m_ChannelID ) - { - continue; - } - - if( rChannelInfoExtra.m_Unit == "meter" ) - { - o_rDeviceOutputUnit = Unit::Meter; - } - else if( rChannelInfoExtra.m_Unit == "kilogram" ) - { - o_rDeviceOutputUnit = Unit::Kilogram; - } - else if( rChannelInfoExtra.m_Unit == "second" ) - { - o_rDeviceOutputUnit = Unit::Second; - } - else if( rChannelInfoExtra.m_Unit == "ampere" ) - { - o_rDeviceOutputUnit = Unit::Ampere; - } - else if( rChannelInfoExtra.m_Unit == "kelvin" ) - { - o_rDeviceOutputUnit = Unit::Kelvin; - } - else if( rChannelInfoExtra.m_Unit == "mole" ) - { - o_rDeviceOutputUnit = Unit::Mole; - } - else if( rChannelInfoExtra.m_Unit == "candela" ) - { - o_rDeviceOutputUnit = Unit::Candela; - } - else if( rChannelInfoExtra.m_Unit == "radian" ) - { - o_rDeviceOutputUnit = Unit::Radian; - } - else if( rChannelInfoExtra.m_Unit == "steradian" ) - { - o_rDeviceOutputUnit = Unit::Steradian; - } - else if( rChannelInfoExtra.m_Unit == "meter squared" ) - { - o_rDeviceOutputUnit = Unit::MeterSquared; - } - else if( rChannelInfoExtra.m_Unit == "meter cubed" ) - { - o_rDeviceOutputUnit = Unit::MeterCubed; - } - else if( rChannelInfoExtra.m_Unit == "meter per second" ) - { - o_rDeviceOutputUnit = Unit::MeterPerSecond; - } - else if( rChannelInfoExtra.m_Unit == "meter per second squared" ) - { - o_rDeviceOutputUnit = Unit::MeterPerSecondSquared; - } - else if( rChannelInfoExtra.m_Unit == "radian per second" ) - { - o_rDeviceOutputUnit = Unit::RadianPerSecond; - } - else if( rChannelInfoExtra.m_Unit == "radian per second squared" ) - { - o_rDeviceOutputUnit = Unit::RadianPerSecondSquared; - } - else if( rChannelInfoExtra.m_Unit == "hertz" ) - { - o_rDeviceOutputUnit = Unit::Hertz; - } - else if( rChannelInfoExtra.m_Unit == "newton" ) - { - o_rDeviceOutputUnit = Unit::Newton; - } - else if( rChannelInfoExtra.m_Unit == "joule" ) - { - o_rDeviceOutputUnit = Unit::Joule; - } - else if( rChannelInfoExtra.m_Unit == "watt" ) - { - o_rDeviceOutputUnit = Unit::Watt; - } - else if( rChannelInfoExtra.m_Unit == "pascal" ) - { - o_rDeviceOutputUnit = Unit::Pascal; - } - else if( rChannelInfoExtra.m_Unit == "lumen" ) - { - o_rDeviceOutputUnit = Unit::Lumen; - } - else if( rChannelInfoExtra.m_Unit == "lux" ) - { - o_rDeviceOutputUnit = Unit::Lux; - } - else if( rChannelInfoExtra.m_Unit == "coulomb" ) - { - o_rDeviceOutputUnit = Unit::Coulomb; - } - else if( rChannelInfoExtra.m_Unit == "volt" ) - { - o_rDeviceOutputUnit = Unit::Volt; - } - else if( rChannelInfoExtra.m_Unit == "ohm" ) - { - o_rDeviceOutputUnit = Unit::Ohm; - } - else if( rChannelInfoExtra.m_Unit == "farad" ) - { - o_rDeviceOutputUnit = Unit::Farad; - } - else if( rChannelInfoExtra.m_Unit == "weber" ) - { - o_rDeviceOutputUnit = Unit::Weber; - } - else if( rChannelInfoExtra.m_Unit == "tesla" ) - { - o_rDeviceOutputUnit = Unit::Tesla; - } - else if( rChannelInfoExtra.m_Unit == "henry" ) - { - o_rDeviceOutputUnit = Unit::Henry; - } - else if( rChannelInfoExtra.m_Unit == "siemens" ) - { - o_rDeviceOutputUnit = Unit::Siemens; - } - else if( rChannelInfoExtra.m_Unit == "becquerel" ) - { - o_rDeviceOutputUnit = Unit::Becquerel; - } - else if( rChannelInfoExtra.m_Unit == "gray" ) - { - o_rDeviceOutputUnit = Unit::Gray; - } - else if( rChannelInfoExtra.m_Unit == "sievert" ) - { - o_rDeviceOutputUnit = Unit::Sievert; - } - else if( rChannelInfoExtra.m_Unit == "katal" ) - { - o_rDeviceOutputUnit = Unit::Katal; - } - - } - } - - return Result::Success; - } - else - { - ++CurrentDeviceOutputIndex; - } - } - } - return Result::InvalidIndex; -} - -Result::Enum VClient::GetDeviceOutputValue( const std::string & i_rDeviceName, - const std::string & i_rDeviceOutputName, - double & o_rValue, - bool & o_rbOccluded ) const -{ - return GetDeviceOutputValue( i_rDeviceName, i_rDeviceOutputName, 0, o_rValue, o_rbOccluded ); -} - - - -Result::Enum VClient::GetDeviceOutputSubsamples( const std::string & i_rDeviceName, - const std::string & i_rDeviceOutputName, - unsigned int & o_rDeviceOutputSubsamples, - bool & o_rbOccluded ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rDeviceOutputSubsamples ) ) - { - return GetResult; - } - - const ViconCGStream::VDeviceInfo * pDevice = GetDevice( i_rDeviceName, GetResult ); - if ( !pDevice ) - { - return GetResult; - } - const unsigned int DeviceID = pDevice->m_DeviceID; - const ViconCGStreamType::UInt64 DevicePeriod = pDevice->m_FramePeriod; - - ViconCGStreamType::Int64 DeviceStartTick = GetDeviceStartTick( DeviceID ); - - const TPeriod FramePeriod = GetFramePeriod( m_LatestFrame ); - - // Iterate over the channels for this device - unsigned int CurrentDeviceOutputIndex = 0; - std::vector< ViconCGStream::VChannelInfo >::const_iterator ChannelIt = m_LatestFrame.m_Channels.begin(); - std::vector< ViconCGStream::VChannelInfo >::const_iterator ChannelEnd = m_LatestFrame.m_Channels.end(); - for( ; ChannelIt != ChannelEnd ; ++ChannelIt ) - { - const ViconCGStream::VChannelInfo & rChannel( *ChannelIt ); - - // Wrong device - if( rChannel.m_DeviceID != DeviceID ) - { - continue; - } - - if( rChannel.m_ComponentNames.empty() ) - { - return Result::NoFrame; - } - - std::vector< std::string >::const_iterator ComponentIt = rChannel.m_ComponentNames.begin(); - std::vector< std::string >::const_iterator ComponentEnd = rChannel.m_ComponentNames.end(); - for( unsigned int ComponentIndex = 0 ; ComponentIt != ComponentEnd ; ++ComponentIt, ++ComponentIndex, ++CurrentDeviceOutputIndex ) - { - if( i_rDeviceOutputName == AdaptDeviceOutputName( *ComponentIt, CurrentDeviceOutputIndex ) ) - { - // This is the correct component. - // Find the correct data array. - - if( IsForcePlateForceChannel( rChannel ) ) - { - o_rbOccluded = !GetSampleCount( m_LatestFrame.m_Forces, rChannel, DevicePeriod, DeviceStartTick, FramePeriod, o_rDeviceOutputSubsamples ); - } - else if( IsForcePlateMomentChannel( rChannel ) ) - { - o_rbOccluded = !GetSampleCount( m_LatestFrame.m_Moments, rChannel, DevicePeriod, DeviceStartTick, FramePeriod, o_rDeviceOutputSubsamples ); - } - else if( IsForcePlateCoPChannel( rChannel ) ) - { - o_rbOccluded = !GetSampleCount( m_LatestFrame.m_CentresOfPressure, rChannel, DevicePeriod, DeviceStartTick, FramePeriod, o_rDeviceOutputSubsamples ); - } - else - { - o_rbOccluded = !GetSampleCount( m_LatestFrame.m_Voltages, rChannel, DevicePeriod, DeviceStartTick, FramePeriod, o_rDeviceOutputSubsamples ); - } - - return Result::Success; - } - } - } - - return Result::InvalidDeviceOutputName; -} - -Result::Enum VClient::GetDeviceOutputValue( const std::string & i_rDeviceName, - const std::string & i_rDeviceOutputName, - unsigned int i_Subsample, - double & o_rValue, - bool & o_rbOccluded ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rValue, o_rbOccluded ) ) - { - return GetResult; - } - - const ViconCGStream::VDeviceInfo * pDevice = GetDevice( i_rDeviceName, GetResult ); - if ( !pDevice ) - { - return GetResult; - } - const unsigned int DeviceID = pDevice->m_DeviceID; - const ViconCGStreamType::UInt64 DevicePeriod = pDevice->m_FramePeriod; - - - ViconCGStreamType::Int64 DeviceStartTick = GetDeviceStartTick( DeviceID ); - - const TPeriod FramePeriod = GetFramePeriod( m_LatestFrame ); - - // Try and find the channel which contains this device output - unsigned int CurrentDeviceOutputIndex = 0; - std::vector< ViconCGStream::VChannelInfo >::const_iterator ChannelIt = m_LatestFrame.m_Channels.begin(); - std::vector< ViconCGStream::VChannelInfo >::const_iterator ChannelEnd = m_LatestFrame.m_Channels.end(); - for( ; ChannelIt != ChannelEnd ; ++ChannelIt ) - { - const ViconCGStream::VChannelInfo & rChannel( *ChannelIt ); - - // Wrong device - if( rChannel.m_DeviceID != DeviceID ) - { - continue; - } - - std::vector< std::string >::const_iterator ComponentIt = rChannel.m_ComponentNames.begin(); - std::vector< std::string >::const_iterator ComponentEnd = rChannel.m_ComponentNames.end(); - for( unsigned int ComponentIndex = 0 ; ComponentIt != ComponentEnd ; ++ComponentIt, ++ComponentIndex, ++CurrentDeviceOutputIndex ) - { - if( i_rDeviceOutputName == AdaptDeviceOutputName( *ComponentIt, CurrentDeviceOutputIndex ) ) - { - // This is the correct component. Now figure out where to get the data from - - // Force frame - if( IsForcePlateForceChannel( rChannel ) ) - { - if( 3 != rChannel.m_ComponentNames.size() ) - { - return Result::Unknown; - } - - double Samples[ 3 ]; - if( !GetSamples( m_LatestFrame.m_Forces, - rChannel, - i_Subsample, - DevicePeriod, - DeviceStartTick, - FramePeriod, - Samples, - o_rbOccluded ) ) - { - return Result::InvalidIndex; - } - - if( !o_rbOccluded ) - { - double TransformedSamples[ 3 ]; - CopyAndTransformT( Samples, TransformedSamples ); - o_rValue = TransformedSamples[ ComponentIndex ]; - } - - return Result::Success; - } - - if( IsForcePlateMomentChannel( rChannel ) ) - { - if( 3 != rChannel.m_ComponentNames.size() ) - { - return Result::Unknown; - } - - double Samples[ 3 ]; - if( !GetSamples( m_LatestFrame.m_Moments, - rChannel, - i_Subsample, - DevicePeriod, - DeviceStartTick, - FramePeriod, - Samples, - o_rbOccluded ) ) - { - return Result::InvalidIndex; - } - - if( !o_rbOccluded ) - { - double TransformedSamples[ 3 ]; - CopyAndTransformT( Samples, TransformedSamples ); - o_rValue = TransformedSamples[ ComponentIndex ]; - } - - return Result::Success; - } - - if( IsForcePlateCoPChannel( rChannel ) ) - { - if( 3 != rChannel.m_ComponentNames.size() ) - { - return Result::Unknown; - } - - double Samples[ 3 ]; - if( !GetSamples( m_LatestFrame.m_CentresOfPressure, - rChannel, - i_Subsample, - DevicePeriod, - DeviceStartTick, - FramePeriod, - Samples, - o_rbOccluded ) ) - { - return Result::InvalidIndex; - } - - if( !o_rbOccluded ) - { - double TransformedSamples[ 3 ]; - CopyAndTransformT( Samples, TransformedSamples ); - o_rValue = TransformedSamples[ ComponentIndex ]; - } - - return Result::Success; - } - - // Voltage - { - std::vector< double > Samples; - if( !GetSamples( m_LatestFrame.m_Voltages, - rChannel, - i_Subsample, - DevicePeriod, - DeviceStartTick, - FramePeriod, - Samples, - o_rbOccluded ) ) - { - return Result::InvalidIndex; - } - - if( !o_rbOccluded ) - { - if( Samples.size() % rChannel.m_ComponentNames.size() != 0 ) - { - return Result::Unknown; - } - - o_rValue = Samples[ ComponentIndex ]; - return Result::Success; - } - } - - // Other - { - // If we didn't find any data for it then we must be occluded - o_rbOccluded = true; - return Result::Success; - } - } - } - } - - return Result::InvalidDeviceOutputName; -} - -Result::Enum VClient::GetCameraCount( unsigned int & o_rCount ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( InitGet( GetResult, o_rCount ) ) - { - o_rCount = static_cast< unsigned int >( m_LatestFrame.m_Cameras.size() ); - } - return GetResult; -} - -Result::Enum VClient::GetCameraName( const unsigned int i_CameraIndex, std::string & o_rCameraName ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( !InitGet( GetResult, o_rCameraName ) ) - { - return GetResult; - } - - if( i_CameraIndex >= m_LatestFrame.m_Cameras.size() ) - { - return Result::InvalidIndex; - } - - const ViconCGStream::VCameraInfo & rCamera( m_LatestFrame.m_Cameras[ i_CameraIndex ] ); - o_rCameraName = AdaptCameraName( rCamera.m_Name, rCamera.m_DisplayType, rCamera.m_CameraID ); - - return GetResult; - -} - -const ViconCGStream::VCameraInfo * VClient::GetCamera( const std::string & i_rCameraName, Result::Enum & o_rResult ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - const auto rCameraIt = - std::find_if( m_LatestFrame.m_Cameras.begin(), m_LatestFrame.m_Cameras.end(), - [&i_rCameraName]( const ViconCGStream::VCameraInfo & rCamera ) - { return AdaptCameraName( rCamera.m_Name, rCamera.m_DisplayType, rCamera.m_CameraID ) == i_rCameraName; } - ); - - if( rCameraIt != m_LatestFrame.m_Cameras.end() ) - { - o_rResult = Result::Success; - return &(*rCameraIt); - } - - o_rResult = Result::InvalidCameraName; - return nullptr; -} - - -Result::Enum VClient::GetCameraID( const std::string & i_rCameraName, unsigned int & o_rCameraID ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Clear( o_rCameraID ); - - Result::Enum GetResult = Result::Success; - const ViconCGStream::VCameraInfo * pCamera = GetCamera( i_rCameraName, GetResult ); - if ( pCamera ) - { - o_rCameraID = pCamera->m_CameraID; - } - return GetResult; -} - -Result::Enum VClient::GetCameraUserID( const std::string & i_rCameraName, unsigned int & o_rUserID ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Clear( o_rUserID ); - - Result::Enum GetResult = Result::Success; - const ViconCGStream::VCameraInfo * pCamera = GetCamera( i_rCameraName, GetResult ); - if ( pCamera ) - { - o_rUserID = pCamera->m_UserID; - } - return GetResult; -} - -Result::Enum VClient::GetCameraType( const std::string & i_rCameraName, std::string & o_rCameraType ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - const ViconCGStream::VCameraInfo * pCamera = GetCamera( i_rCameraName, GetResult ); - if ( pCamera ) - { - o_rCameraType = pCamera->m_DisplayType; - } - - return GetResult; -} - - -Result::Enum VClient::GetCameraDisplayName( const std::string & i_rCameraName, std::string & o_rCameraDisplayName ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - const ViconCGStream::VCameraInfo * pCamera = GetCamera( i_rCameraName, GetResult ); - if ( pCamera ) - { - o_rCameraDisplayName = pCamera->m_Name; - } - - return GetResult; -} - - -Result::Enum VClient::GetCameraResolution( const std::string & i_rCameraName, unsigned int & o_rResolutionX, unsigned int & o_rResolutionY ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if( InitGet( GetResult, o_rResolutionX, o_rResolutionY ) ) - { - const ViconCGStream::VCameraInfo* pCamera = GetCamera( i_rCameraName, GetResult ); - if( !pCamera ) - { - return GetResult; - } - - o_rResolutionX = pCamera->m_ResolutionX; - o_rResolutionY = pCamera->m_ResolutionY; - } - - return GetResult; -} - -Result::Enum VClient::GetIsVideoCamera( const std::string & i_rCameraName, bool & o_rIsVideoCamera ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if( InitGet( GetResult, o_rIsVideoCamera ) ) - { - const ViconCGStream::VCameraInfo* pCamera = GetCamera( i_rCameraName, GetResult ); - if( !pCamera ) - { - return GetResult; - } - - o_rIsVideoCamera = pCamera->m_bIsVideoCamera; - } - - return GetResult; -} - - -Result::Enum VClient::GetCentroidCount( const std::string & i_rCameraName, unsigned int & o_rCount ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if( InitGet( GetResult, o_rCount ) ) - { - const ViconCGStream::VCameraInfo* pCamera = GetCamera( i_rCameraName, GetResult ); - if( !pCamera ) - { - return GetResult; - } - - const ViconCGStream::VCentroids* pCentroidSet = GetCentroidSet( pCamera->m_CameraID, GetResult ); - if( pCentroidSet ) - { - o_rCount = static_cast< unsigned int >( pCentroidSet->m_Centroids.size() ); - } - } - return GetResult; -} - -Result::Enum VClient::GetCentroidPosition( const std::string & i_rCameraName, - const unsigned int i_CentroidIndex, - double (&o_rPosition)[2], - double & o_rRadius /*, - double & o_rAccuracy */ ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( InitGet( GetResult, o_rPosition, o_rRadius /*, o_rAccuracy */ ) ) - { - const ViconCGStream::VCameraInfo* pCamera = GetCamera( i_rCameraName, GetResult ); - if( !pCamera ) - { - return GetResult; - } - - const ViconCGStream::VCentroids* pCentroidSet = GetCentroidSet( pCamera->m_CameraID, GetResult ); - if( pCentroidSet ) - { - if( i_CentroidIndex < pCentroidSet->m_Centroids.size() ) - { - const ViconCGStreamDetail::VCentroids_Centroid & rCentroid = - pCentroidSet->m_Centroids[ i_CentroidIndex ]; - - o_rPosition[0] = rCentroid.m_Position[0]; - o_rPosition[1] = rCentroid.m_Position[1]; - o_rRadius = rCentroid.m_Radius; -// o_rAccuracy = rCentroid.m_Accuracy; - } - else - { - GetResult = Result::InvalidIndex; - } - } - } - return GetResult; -} - -Result::Enum VClient::GetCentroidWeight( const std::string & i_rCameraName, - const unsigned int i_CentroidIndex, - double & o_rWeight ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if( InitGet( GetResult, o_rWeight ) ) - { - const ViconCGStream::VCameraInfo* pCamera = GetCamera( i_rCameraName, GetResult ); - if( !pCamera ) - { - return GetResult; - } - - const ViconCGStream::VCentroidWeights* pCentroidWeightSet = GetCentroidWeightSet( pCamera->m_CameraID, GetResult ); - if( pCentroidWeightSet ) - { - if( i_CentroidIndex < pCentroidWeightSet->m_Weights.size() ) - { - o_rWeight = pCentroidWeightSet->m_Weights[i_CentroidIndex];; - } - else - { - GetResult = Result::InvalidIndex; - } - } - } - return GetResult; -} - -Result::Enum VClient::GetGreyscaleBlobCount( const std::string & i_rCameraName, unsigned int & o_rCount ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if( InitGet( GetResult, o_rCount ) ) - { - const ViconCGStream::VCameraInfo* pCamera = GetCamera( i_rCameraName, GetResult ); - if( !pCamera ) - { - return GetResult; - } - - const ViconCGStream::VGreyscaleBlobs * pBlobSet = GetGreyscaleBlobs( pCamera->m_CameraID, GetResult ); - if( pBlobSet ) - { - o_rCount = static_cast< unsigned int >( pBlobSet->m_GreyscaleBlobs.size() ); - } - } - return GetResult; -} - - -Result::Enum VClient::GetGreyscaleBlob( const std::string & i_rCameraName, - const unsigned int i_BlobIndex, - std::vector< unsigned int > & o_rLineXPositions, - std::vector< unsigned int > & o_rLineYPositions, - std::vector< std::vector< unsigned char > > & o_rLinePixelValues ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( InitGet( GetResult, o_rLineXPositions, o_rLineYPositions, o_rLinePixelValues ) ) - { - const ViconCGStream::VCameraInfo* pCamera = GetCamera( i_rCameraName, GetResult ); - if( !pCamera ) - { - return GetResult; - } - - const ViconCGStream::VGreyscaleBlobs* pBlobSet = GetGreyscaleBlobs( pCamera->m_CameraID, GetResult ); - if( pBlobSet ) - { - if( i_BlobIndex < pBlobSet->m_GreyscaleBlobs.size() ) - { - const ViconCGStreamDetail::VGreyscaleBlobs_GreyscaleBlob & rBlob = pBlobSet->m_GreyscaleBlobs[ i_BlobIndex ]; - - o_rLineXPositions.reserve( rBlob.m_GreyscaleLines.size() ); - o_rLineYPositions.reserve( rBlob.m_GreyscaleLines.size() ); - o_rLinePixelValues.reserve( rBlob.m_GreyscaleLines.size() ); - - for ( auto line : rBlob.m_GreyscaleLines ) - { - o_rLineXPositions.push_back( line.m_Position[0] ); - o_rLineYPositions.push_back( line.m_Position[1] ); - o_rLinePixelValues.push_back( line.m_Greyscale); - } - - } - else - { - GetResult = Result::InvalidIndex; - } - } - } - - return GetResult; -} - -Result::Enum VClient::GetVideoFrame( const std::string & i_rCameraName, ViconCGStreamClientSDK::VVideoFramePtr & o_rVideoFramePtr ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - Result::Enum GetResult = Result::Success; - if ( InitGet( GetResult ) ) - { - const ViconCGStream::VCameraInfo* pCamera = GetCamera( i_rCameraName, GetResult ); - if( !pCamera ) - { - return GetResult; - } - - GetVideoFrame( pCamera->m_CameraID, GetResult, o_rVideoFramePtr ); - } - - return GetResult; -} - -Result::Enum VClient::SetCameraFilter( const std::vector< unsigned int > & i_rCameraIdsForCentroids, - const std::vector< unsigned int > & i_rCameraIdsForBlobs, - const std::vector< unsigned int > & i_rCameraIdsForVideo ) -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - // Clear any current filters on supported camera data types - m_Filter.Clear( ViconCGStreamEnum::Centroids ); - m_Filter.Clear( ViconCGStreamEnum::GreyscaleBlobs ); - m_Filter.Clear( ViconCGStreamEnum::VideoFrame ); - - // Add new filters for the given cameras - for ( unsigned int Id : i_rCameraIdsForCentroids ) - { - m_Filter.Add( ViconCGStreamEnum::Centroids, Id ); - } - - for ( unsigned int Id : i_rCameraIdsForBlobs ) - { - m_Filter.Add( ViconCGStreamEnum::GreyscaleBlobs, Id ); - } - - for ( unsigned int Id : i_rCameraIdsForVideo ) - { - m_Filter.Add( ViconCGStreamEnum::VideoFrame, Id ); - } - - // Items with no filter set in the CGServer are allowed by default. If any of the input - // vectors are empty, the client request is that no data is sent for that type. We have - // to add a dummy id in this case to activate the filter and circumvent its default pass - m_Filter.Add( ViconCGStreamEnum::Centroids, -1 ); - m_Filter.Add( ViconCGStreamEnum::GreyscaleBlobs, -1 ); - m_Filter.Add( ViconCGStreamEnum::VideoFrame, -1 ); - - m_pClient->SetFilter( m_Filter ); - - return Result::Success; -} - - -ViconCGStreamClientSDK::ICGFrameState& VClient::LatestFrame() -{ - return m_LatestFrame; -} - -ViconCGStreamClientSDK::ICGFrameState& VClient::CachedFrame() -{ - return m_CachedFrame; -} - -void VClient::CopyAndTransformT( const double i_Translation[ 3 ], double ( & io_Translation )[ 3 ] ) const -{ - if (m_pAxisMapping) - { - - Direction::Enum RequestedX, RequestedY, RequestedZ; - m_pAxisMapping->GetAxisMapping(RequestedX, RequestedY, RequestedZ); - - Direction::Enum ServerX, ServerY, ServerZ; - - AxisMappingResult::Enum Error = AxisMappingResult::Success; - std::shared_ptr< VAxisMapping > pServerAxisMapping; - - // If we've got information about our stream type from the server - if (m_CachedFrame.m_ApplicationInfo && m_CachedFrame.m_ApplicationInfo.get().m_AxisOrientation == ViconCGStream::VApplicationInfo::EYUp) - { - ServerX = Direction::Forward; - ServerY = Direction::Up; - ServerZ = Direction::Right; - - // Only create the server axis mapping if we need to modify the data from the server - pServerAxisMapping = VAxisMapping::Create(Error, Direction::Forward, Direction::Up, Direction::Right); - } - else - { - // We either know it's Z-up, or we assume it's Z-up due to lack of contrary information. - ServerX = Direction::Forward; - ServerY = Direction::Left; - ServerZ = Direction::Up; - } - - // We will avoid the mapping in favour of a pure copy if the input and requested output are the same. - if (RequestedX == ServerX && RequestedY == ServerY && RequestedZ == ServerZ) - { - std::copy(i_Translation, i_Translation + 3, io_Translation); - } - else - { - if (pServerAxisMapping && Error == AxisMappingResult::Success) - { - double Q[9]; - pServerAxisMapping->GetTransformationMatrix(Q); - m_pAxisMapping->CopyAndTransformT(i_Translation, Q, io_Translation); - } - else - { - // We don't need to do a server axis mapping - m_pAxisMapping->CopyAndTransformT(i_Translation, io_Translation); - } - } - } - else - { - // Just do a pure copy if there's no axis mapping at all. - std::copy( i_Translation, i_Translation + 3, io_Translation ); - } -} - -void VClient::CopyAndTransformR( const double i_Rotation[ 9 ], double ( & io_Rotation )[ 9 ] ) const -{ - if (m_pAxisMapping) - { - - Direction::Enum RequestedX, RequestedY, RequestedZ; - m_pAxisMapping->GetAxisMapping(RequestedX, RequestedY, RequestedZ); - - Direction::Enum ServerX, ServerY, ServerZ; - - AxisMappingResult::Enum Error = AxisMappingResult::Success; - std::shared_ptr< VAxisMapping > pServerAxisMapping; - - // If we've got information about our stream type from the server - if (m_CachedFrame.m_ApplicationInfo && m_CachedFrame.m_ApplicationInfo.get().m_AxisOrientation == ViconCGStream::VApplicationInfo::EYUp) - { - ServerX = Direction::Forward; - ServerY = Direction::Up; - ServerZ = Direction::Right; - - // Only create the server axis mapping if we need to modify the data from the server - pServerAxisMapping = VAxisMapping::Create(Error, Direction::Forward, Direction::Up, Direction::Right); - } - else - { - // We either know it's Z-up, or we assume it's Z-up due to lack of contrary information. - ServerX = Direction::Forward; - ServerY = Direction::Left; - ServerZ = Direction::Up; - } - - // We will avoid the mapping in favour of a pure copy if the input and requested output are the same. - if (RequestedX == ServerX && RequestedY == ServerY && RequestedZ == ServerZ) - { - std::copy(i_Rotation, i_Rotation + 9, io_Rotation); - } - else - { - if (pServerAxisMapping && Error == AxisMappingResult::Success) - { - double Q[9]; - pServerAxisMapping->GetTransformationMatrix(Q); - m_pAxisMapping->CopyAndTransformR(i_Rotation, Q, io_Rotation); - } - else - { - // We don't need to do a server axis mapping - m_pAxisMapping->CopyAndTransformR(i_Rotation, io_Rotation); - } - } - } - else - { - // Just do a pure copy if there's no axis mapping at all. - std::copy(i_Rotation, i_Rotation + 9, io_Rotation); - } - -} - -ViconCGStreamType::UInt64 VClient::GetDevicePeriod( const unsigned int i_DeviceID ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - std::vector< ViconCGStream::VDeviceInfo >::const_iterator It = m_LatestFrame.m_Devices.begin(); - std::vector< ViconCGStream::VDeviceInfo >::const_iterator End = m_LatestFrame.m_Devices.end(); - for( ; It != End ; ++It ) - { - const ViconCGStream::VDeviceInfo & rDevice( *It ); - if ( rDevice.m_DeviceID == i_DeviceID ) - { - return rDevice.m_FramePeriod; - } - } - return 0; -} - -ViconCGStreamType::UInt64 VClient::GetDeviceStartTick( const unsigned int i_DeviceID ) const -{ - boost::recursive_mutex::scoped_lock Lock( m_FrameMutex ); - - std::vector< ViconCGStream::VDeviceInfoExtra >::const_iterator It = m_LatestFrame.m_DevicesExtra.begin(); - std::vector< ViconCGStream::VDeviceInfoExtra >::const_iterator End = m_LatestFrame.m_DevicesExtra.end(); - for( ; It != End ; ++It ) - { - const ViconCGStream::VDeviceInfoExtra & rDevice( *It ); - if ( rDevice.m_DeviceID == i_DeviceID ) - { - return rDevice.m_FrameStartTick; - } - } - return 0; -} -} // End of namespace Core -} // End of namespace ViconDataStreamSDK diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/CoreClient.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/CoreClient.h deleted file mode 100644 index 4d7b346c9..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/CoreClient.h +++ /dev/null @@ -1,449 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -#include "Constants.h" -#include "AxisMapping.h" -#include "ClientUtils.h" -#include "RetimingClient.h" -#include -#include -#include -#include -#include -#include -#include - - -namespace ViconDataStreamSDK -{ -namespace Core -{ - -class VAxisMapping; - -class VClient -{ -public: - - VClient(); - virtual ~VClient(); - - // Get the version of the Vicon Data Stream SDK - void GetVersion( unsigned int & o_rMajor, - unsigned int & o_rMinor, - unsigned int & o_rPoint ) const; - - // Connect client to the Vicon Data Stream - Result::Enum Connect( std::shared_ptr< ViconCGStreamClientSDK::ICGClient > i_pClient, - const std::string & i_rHostName ); - - // Connect client to the Vicon Data Stream - Result::Enum ConnectToMulticast( std::shared_ptr< ViconCGStreamClientSDK::ICGClient > i_pClient, - const std::string & i_rLocalIP, - const std::string & i_rMulticastIP ); - - // Disconnect from the Vicon Data Stream - Result::Enum Disconnect(); - - // Multicast transmission - Result::Enum StartTransmittingMulticast( const std::string & i_rServerIP, - const std::string & i_rMulticastIP ); - - Result::Enum StopTransmittingMulticast(); - - // states whether the stream is connected or not - bool IsConnected() const; - - // Control how many frames are buffered by the client (default is one) - void SetBufferSize( unsigned int i_MaxFrames ); - - Result::Enum GetFrame(); - Result::Enum GetFrameNumber( unsigned int & o_rFrameNumber ) const; - Result::Enum GetFrameRate( double & o_rFrameRateInHz ) const; - - Result::Enum GetTimecode( unsigned int & o_rHours, - unsigned int & o_rMinutes, - unsigned int & o_rSeconds, - unsigned int & o_rFrames, - unsigned int & o_rSubFrame, - bool & o_rbFieldFlag, - TimecodeStandard::Enum & o_rTimecodeStandard, - unsigned int & o_rSubFramesPerFrame, - unsigned int & o_rUserBits ) const; - - - // Latency reporting - Result::Enum GetNetworkLatency(double & o_rLatency); - Result::Enum GetLatencyTotal( double & o_rLatency ) const; - Result::Enum GetLatencySampleCount( unsigned int & o_rSampleCount ) const; - Result::Enum GetLatencySampleName( const unsigned int i_SampleIndex, std::string & o_rSampleName ) const; - Result::Enum GetLatencySampleValue( const std::string & i_rSampleName, double & o_rSampleValue ) const; - - // Get native hardware frame (for debugging purposes) - Result::Enum GetHardwareFrameNumber( unsigned int & o_rFrameNumber ) const; - - // Get framerates information - Result::Enum GetFrameRateCount( unsigned int & o_rFrameRateCount ) const; - Result::Enum GetFrameRateName( const unsigned int i_FrameRateIndex, std::string & o_rFrameRateName ) const; - Result::Enum GetFrameRateValue( const std::string & i_rFrameRateName, double & o_rFrameRateValue ) const; - - Result::Enum SetSegmentDataEnabled( const bool i_i_bEnabled ); - Result::Enum SetMarkerDataEnabled( const bool i_bEnabled ); - Result::Enum SetUnlabeledMarkerDataEnabled( const bool i_bEnabled ); - Result::Enum SetMarkerRayDataEnabled( const bool i_bEnabled ); - Result::Enum SetDeviceDataEnabled( const bool i_bEnabled ); - Result::Enum SetCentroidDataEnabled( const bool i_bEnabled ); - Result::Enum SetGreyscaleDataEnabled( const bool i_bEnabled ); - Result::Enum SetDebugDataEnabled( const bool i_bEnabled ); - Result::Enum SetCameraWand2dDataEnabled( const bool i_bEnabled ); - Result::Enum SetVideoDataEnabled( const bool i_bEnabled ); - - Result::Enum EnableSegmentData() { return SetSegmentDataEnabled( true ); } - Result::Enum EnableMarkerData() { return SetMarkerDataEnabled( true ); } - Result::Enum EnableUnlabeledMarkerData() { return SetUnlabeledMarkerDataEnabled( true ); } - Result::Enum EnableMarkerRayData() { return SetMarkerRayDataEnabled( true ); } - Result::Enum EnableDeviceData() { return SetDeviceDataEnabled( true ); } - Result::Enum EnableCentroidData() { return SetCentroidDataEnabled( true ); } - Result::Enum EnableGreyscaleData() { return SetGreyscaleDataEnabled( true ); } - Result::Enum EnableDebugData() { return SetDebugDataEnabled( true ); } - Result::Enum EnableCameraWand2dData() { return SetCameraWand2dDataEnabled( true ); } - Result::Enum EnableVideoData() { return SetVideoDataEnabled( true ); } - - Result::Enum DisableSegmentData() { return SetSegmentDataEnabled( false ); } - Result::Enum DisableMarkerData() { return SetMarkerDataEnabled( false ); } - Result::Enum DisableUnlabeledMarkerData() { return SetUnlabeledMarkerDataEnabled( false ); } - Result::Enum DisableMarkerRayData() { return SetMarkerRayDataEnabled( false ); } - Result::Enum DisableDeviceData() { return SetDeviceDataEnabled( false ); } - Result::Enum DisableCentroidData() { return SetCentroidDataEnabled( false ); } - Result::Enum DisableGreyscaleData() { return SetGreyscaleDataEnabled( false ); } - Result::Enum DisableDebugData() { return SetDebugDataEnabled( false ); } - Result::Enum DisableCameraWand2dData() { return SetCameraWand2dDataEnabled( false ); } - Result::Enum DisableVideoData() { return SetVideoDataEnabled( false ); } - - bool IsSegmentDataEnabled() const; - bool IsMarkerDataEnabled() const; - bool IsUnlabeledMarkerDataEnabled() const; - bool IsMarkerRayDataEnabled() const; - bool IsDeviceDataEnabled() const; - bool IsCentroidDataEnabled() const; - bool IsGreyscaleDataEnabled() const; - bool IsDebugDataEnabled() const; - bool IsCameraWand2dDataEnabled() const; - bool IsVideoDataEnabled() const; - - Result::Enum SetStreamMode( const StreamMode::Enum i_Mode ); - - Result::Enum SetApexDeviceFeedback( const std::string& i_rDeviceName, bool i_bOn ); - - Result::Enum SetAxisMapping( const Direction::Enum i_XAxis, const Direction::Enum i_YAxis, const Direction::Enum i_ZAxis ); - void GetAxisMapping( Direction::Enum & o_rXAxis, Direction::Enum & o_rYAxis, Direction::Enum & o_rZAxis ) const; - - Result::Enum GetServerOrientation( ServerOrientation::Enum & o_rServerOrientation ) const; - Result::Enum GetSubjectCount( unsigned int & o_rSubjectCount ) const; - Result::Enum GetSubjectName( const unsigned int i_SubjectIndex, std::string& o_rSubjectName ) const; - Result::Enum GetSubjectRootSegmentName( const std::string & i_rSubjectName, std::string & o_rSegmentName ) const; - - Result::Enum GetSegmentCount(const std::string& i_rSubjectName, unsigned int& o_rSegmentCount) const; - Result::Enum GetSegmentName( const std::string& i_rSubjectName, const unsigned int i_SegmentIndex, std::string& o_rSegmentName) const; - Result::Enum GetSegmentChildCount( const std::string& i_rSubjectName, const std::string& i_rSegmentName, unsigned int & o_rSegmentCount ) const; - Result::Enum GetSegmentChildName( const std::string& i_rSubjectName, const std::string& i_rSegmentName, unsigned int i_SegmentIndex, std::string& o_rSegmentName ) const; - Result::Enum GetSegmentParentName( const std::string& i_rSubjectName, const std::string& i_rSegmentName, std::string& o_rSegmentName ) const; - - Result::Enum GetSegmentStaticTranslation(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double (&o_rThreeVector)[3] ) const; - Result::Enum GetSegmentStaticRotationHelical(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double (&o_rThreeVector)[3] ) const; - Result::Enum GetSegmentStaticRotationMatrix( const std::string & i_rSubjectName, const std::string & i_rSegmentName, double (& o_rRotation)[9] ) const; - Result::Enum GetSegmentStaticRotationQuaternion(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double (&o_rFourVector)[4] ) const; - Result::Enum GetSegmentStaticRotationEulerXYZ(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double (&o_rThreeVector)[3] ) const; - - Result::Enum GetSegmentGlobalTranslation(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double (&o_rThreeVector)[3], bool& o_rbOccludedFlag) const; - Result::Enum GetSegmentGlobalRotationHelical(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double (&o_rThreeVector)[3], bool& o_rbOccludedFlag) const; - Result::Enum GetSegmentGlobalRotationMatrix( const std::string & i_rSubjectName, const std::string & i_rSegmentName, double (& o_rRotation)[9], bool & o_rbOccluded ) const; - Result::Enum GetSegmentGlobalRotationQuaternion(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double (&o_rFourVector)[4], bool& o_rbOccluded ) const; - Result::Enum GetSegmentGlobalRotationEulerXYZ(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double (&o_rThreeVector)[3], bool& o_rbOccludedFlag) const; - - Result::Enum GetSegmentLocalTranslation(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double (&o_pThreeVector)[3], bool& o_rbOccludedFlag) const; - Result::Enum GetSegmentLocalRotationHelical(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double (&o_pThreeVector)[3], bool& o_rbOccludedFlag) const; - Result::Enum GetSegmentLocalRotationMatrix( const std::string & i_rSubjectName, const std::string & i_rSegmentName, double (& o_rRotation)[9], bool & o_rbOccluded ) const; - Result::Enum GetSegmentLocalRotationQuaternion(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double (&o_pFourVector)[4], bool& o_rbOccludedFlag) const; - Result::Enum GetSegmentLocalRotationEulerXYZ(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double (&o_pThreeVector)[3], bool& o_rbOccludedFlag) const; - - Result::Enum GetObjectQuality( const std::string& i_rObjectName, double& o_rQuality ) const; - Result::Enum GetMarkerCount( const std::string& i_rSubjectName, unsigned int& o_rMarkerCount ) const; - Result::Enum GetMarkerName(const std::string& i_rSubjectName, const unsigned int i_MarkerIndex, std::string& o_rMarkerName) const; - Result::Enum GetMarkerParentName( const std::string & i_rSubjectName, const std::string & i_rMarkerName, std::string & o_rSegmentName ) const; - Result::Enum GetMarkerGlobalTranslation( const std::string& i_rSubjectName, const std::string& i_rMarkerName, double (&o_rThreeVector)[3], bool& o_rbOccludedFlag ) const; - Result::Enum GetMarkerRayAssignmentCount( const std::string& i_rSubjectName, const std::string& i_rMarkerName, unsigned int & o_rAssignmentCount ) const; - Result::Enum GetMarkerRayAssignment( const std::string& i_rSubjectName , const std::string& i_rMarkerName , int i_rAssignmentIndex , unsigned int & o_rCameraID , unsigned int & o_rCentroidIndex ) const; - - Result::Enum GetUnlabeledMarkerCount( unsigned int & o_rMarkerCount ) const; - Result::Enum GetUnlabeledMarkerGlobalTranslation( const unsigned int i_MarkerIndex, - double (& o_rTranslation)[3] ) const; - - Result::Enum GetLabeledMarkerCount( unsigned int & o_rMarkerCount ) const; - Result::Enum GetLabeledMarkerGlobalTranslation( const unsigned int i_MarkerIndex, - double (& o_rTranslation)[3] ) const; - - Result::Enum GetDeviceCount( unsigned int & o_rDeviceCount ) const; - - // Empty device names are automatically assigned a name 'Unnamed Device N' where N is a 1 based device number. - - Result::Enum GetDeviceName( const unsigned int i_DeviceIndex, - std::string & o_rDeviceName, - DeviceType::Enum & o_rDeviceType ) const; - - Result::Enum GetDeviceOutputCount( const std::string & i_rDeviceName, - unsigned int & o_rDeviceOutputCount ) const; - - // Empty device output names are automatically assigned a name 'Unnamed Device Output N' where N is a 1 based device output index. - - Result::Enum GetDeviceOutputName( const std::string & i_rDeviceName, - const unsigned int i_DeviceOutputIndex, - std::string & o_rDeviceOutputName, - Unit::Enum & o_rDeviceOutputUnit ) const; - - // Get the first subsample for the device output. - - Result::Enum GetDeviceOutputValue( const std::string & i_rDeviceName, - const std::string & i_rDeviceOutputName, - double & o_rValue, - bool & o_rbOccluded ) const; - - // Get the number of subsamples for this device output. - - Result::Enum GetDeviceOutputSubsamples( const std::string & i_rDeviceName, - const std::string & i_rDeviceOutputName, - unsigned int & o_rDeviceOutputSubsamples, - bool & o_rbOccluded ) const; - - // Get a specific subsample. - - Result::Enum GetDeviceOutputValue( const std::string & i_rDeviceName, - const std::string & i_rDeviceOutputName, - unsigned int i_Subsample, - double & o_rValue, - bool & o_rbOccluded ) const; - - Result::Enum GetForcePlateCount( unsigned int & o_rCount ) const; - Result::Enum GetForcePlateID( const unsigned int i_ZeroIndexedPlateIndex, unsigned int& o_rPlateID ) const; - - // This set of functions return the first subsample only. - - Result::Enum GetForceVector( const unsigned int i_PlateID, double ( & o_rForceVector)[3] ) const; - Result::Enum GetMomentVector( const unsigned int i_PlateID, double ( & o_rMomentVector)[3] ) const; - Result::Enum GetCentreOfPressure( const unsigned int i_PlateID, double ( & o_rLocation )[3] ) const; - Result::Enum GetGlobalForceVector( const unsigned int i_PlateID, double ( & o_rForceVector)[3] ) const; - Result::Enum GetGlobalMomentVector( const unsigned int i_PlateID, double ( & o_rMomentVector)[3] ) const; - Result::Enum GetGlobalCentreOfPressure( const unsigned int i_PlateID, double ( & o_rLocation )[3] ) const; - - // Returns the number of subsamples available for the plate. - - Result::Enum GetForcePlateSubsamples( const unsigned int i_PlateID, unsigned int& o_rForcePlateSubsamples ) const; - - Result::Enum GetForceVector( const unsigned int i_PlateID, const unsigned int i_Subsample, double ( & o_rForceVector)[3] ) const; - Result::Enum GetMomentVector( const unsigned int i_PlateID, const unsigned int i_Subsample, double ( & o_rMomentVector)[3] ) const; - Result::Enum GetCentreOfPressure( const unsigned int i_PlateID, const unsigned int i_Subsample, double ( & o_rLocation )[3] ) const; - Result::Enum GetGlobalForceVector( const unsigned int i_PlateID, const unsigned int i_Subsample, double ( & o_rForceVector)[3] ) const; - Result::Enum GetGlobalMomentVector( const unsigned int i_PlateID, const unsigned int i_Subsample, double ( & o_rMomentVector)[3] ) const; - Result::Enum GetGlobalCentreOfPressure( const unsigned int i_PlateID, const unsigned int i_Subsample, double ( & o_rLocation )[3] ) const; - - // Get the raw analogue associated with the plate. - - Result::Enum GetNumberOfAnalogChannels( const unsigned int i_PlateID, unsigned int& o_rChannelCount ) const; - Result::Enum GetAnalogChannelVoltage( const unsigned int i_PlateID, const unsigned int i_ZeroIndexedChannelIndex, double& o_rVoltage ) const; - Result::Enum GetAnalogChannelVoltage( const unsigned int i_PlateID, const unsigned int i_ZeroIndexedChannelIndex, const unsigned int i_Subsample, double& o_rVoltage ) const; - - Result::Enum GetEyeTrackerCount( unsigned int & o_rCount ) const; - Result::Enum GetEyeTrackerID( const unsigned int i_EyeTrackerIndex, unsigned int& o_rEyeTrackerID ) const; - Result::Enum GetEyeTrackerGlobalPosition( const unsigned int i_EyeTrackerID, double (&o_rThreeVector)[3], bool& o_rbOccludedFlag ) const; - Result::Enum GetEyeTrackerGlobalGazeVector( const unsigned int i_EyeTrackerID, double (&o_rThreeVector)[3], bool& o_rbOccludedFlag ) const; - - Result::Enum GetCameraCount( unsigned int & o_rCount ) const; - Result::Enum GetCameraName( const unsigned int i_CameraIndex, std::string & o_rCameraName ) const; - Result::Enum GetCameraID( const std::string & i_rCameraName, unsigned int & o_rCameraID ) const; - Result::Enum GetCameraUserID( const std::string & i_rCameraName, unsigned int & o_rCameraID ) const; - Result::Enum GetCameraType( const std::string & i_rCameraName, std::string & o_rCameraType ) const; - Result::Enum GetCameraDisplayName( const std::string & i_rCameraName, std::string & o_rCameraDisplayName ) const; - - Result::Enum GetCameraResolution( const std::string & i_rCameraName, unsigned int & o_rResolutionX, unsigned int & o_rResolutionY ) const; - Result::Enum GetIsVideoCamera( const std::string & i_rCameraName, bool & o_rIsVideoCamera ) const; - - Result::Enum GetCentroidCount( const std::string & i_rCameraName, unsigned int & o_rCount ) const; - Result::Enum GetCentroidPosition( const std::string & i_rCameraName, - const unsigned int i_CentroidIndex, - double (&o_rPosition)[2], - double & o_rRadius /*, - double & o_rAccuracy */ ) const; - Result::Enum GetCentroidWeight( const std::string & i_rCameraName, - const unsigned int i_CentroidIndex, - double & o_rWeight ) const; - - Result::Enum GetGreyscaleBlobCount( const std::string & i_rCameraName, unsigned int & o_rCount ) const; - Result::Enum GetGreyscaleBlob( const std::string & i_rCameraName, - const unsigned int i_BlobIndex, - std::vector< unsigned int > & o_rLineXPositions, - std::vector< unsigned int > & o_rLineYPositions, - std::vector< std::vector< unsigned char > > & o_rLinePixelValues ) const; - - Result::Enum GetVideoFrame( const std::string & i_rCameraName, ViconCGStreamClientSDK::VVideoFramePtr & o_rVideoFramePtr ) const; - - // Only stream camera data for the given cameras - Result::Enum SetCameraFilter( const std::vector< unsigned int > & i_rCameraIdsForCentroids, - const std::vector< unsigned int > & i_rCameraIdsForBlobs, - const std::vector< unsigned int > & i_rCameraIdsForVideo ); - - ViconCGStreamClientSDK::ICGFrameState& LatestFrame(); - ViconCGStreamClientSDK::ICGFrameState& CachedFrame(); - -private: - Result::Enum GetSubjectAndMarkerID(const std::string& i_rSubjectName, const std::string& i_rMarkerName, unsigned int& o_rSubjectID, unsigned int& o_rMarkerID) const; - Result::Enum GetSubjectAndSegmentID(const std::string& i_rSubjectName, const std::string& i_rSegmentName, unsigned int& o_rSubjectID, unsigned int& o_rSegmentID) const; - Result::Enum GetDeviceID( const std::string & i_rDeviceName, unsigned int & o_rDeviceID ) const; - Result::Enum GetReconRayAssignments( const std::string& i_rSubjectName , const std::string& i_rMarkerName , std::vector< unsigned int >& o_rCameraIDs , std::vector< unsigned int >& o_rCentroidIndex ) const; - - const ViconCGStream::VSubjectInfo * GetSubjectInfo( const std::string i_rSubjectName, Result::Enum & o_rResult ) const; - const ViconCGStream::VSubjectTopology * GetSubjectTopology( const unsigned int i_SubjectID ) const; - const ViconCGStream::VObjectQuality * GetObjectQuality( const unsigned int i_SubjectID ) const; - const ViconCGStream::VDeviceInfo * GetDevice( const std::string & i_rDeviceName, Result::Enum & o_rResult ) const; - const ViconCGStream::VCameraInfo * GetCamera( const std::string & i_rCameraName, Result::Enum & o_rResult ) const; - const ViconCGStream::VCentroids * GetCentroidSet( const unsigned int i_CameraID, Result::Enum & o_rResult ) const; - const ViconCGStream::VCentroidWeights * GetCentroidWeightSet( const unsigned int i_CameraID, Result::Enum & o_rResult ) const; - const ViconCGStream::VGreyscaleBlobs * GetGreyscaleBlobs( const unsigned int i_CameraID, Result::Enum & o_rResult ) const; - void GetVideoFrame( const unsigned int i_CameraID, Result::Enum & o_rResult, ViconCGStreamClientSDK::VVideoFramePtr & o_rVideoFramePtr ) const; - - Result::Enum GetMarkerID( const ViconCGStream::VSubjectInfo & i_rSubjectInfo, const std::string& i_rMarkerName, unsigned int& o_rMarkerID ) const; - Result::Enum GetSegmentID( const ViconCGStream::VSubjectInfo & i_rSubjectInfo, const std::string& i_rSegmentName, unsigned int& o_rSegmentID ) const; - - bool IsForcePlateCoreChannel(const ViconCGStream::VChannelInfo& rChannel) const; - bool IsForcePlateForceChannel(const ViconCGStream::VChannelInfo& rChannel) const; - bool IsForcePlateMomentChannel(const ViconCGStream::VChannelInfo& rChannel) const; - bool IsForcePlateCoPChannel(const ViconCGStream::VChannelInfo& rChannel) const; - bool IsForcePlateDevice(unsigned int i_DeviceID) const; - - bool ForcePlateDeviceIndex( unsigned int i_DeviceID, unsigned int & o_rZeroBasedIndex ) const; - - Result::Enum ForcePlateSubsamples( unsigned int i_PlateID, unsigned int & o_rForcePlateSubsamples ) const; - template < typename T > Result::Enum GetForcePlateVector( const unsigned int i_PlateID, - const unsigned int i_ForcePlateSubsamples, - const std::vector< T > & i_rFrameVector, - boost::array< double, 3 > & o_rForcePlateVector ) const; - Result::Enum GetForceVector( const unsigned int i_PlateID, const unsigned int i_ForcePlateSubsamples, boost::array< double, 3 > & o_rForceVector ) const; - Result::Enum GetMomentVector( const unsigned int i_PlateID, const unsigned int i_ForcePlateSubsamples, boost::array< double, 3 > & o_rMomentVector ) const; - Result::Enum GetCentreOfPressure( const unsigned int i_PlateID, const unsigned int i_ForcePlateSubsamples, boost::array< double, 3 > & o_rLocation ) const; - - bool InitGet( Result::Enum & o_rResult ) const; - template < typename T > bool InitGet( Result::Enum & o_rResult, T & o_rOutput ) const; - template < typename T1, typename T2 > bool InitGet( Result::Enum & o_rResult, T1 & o_rOutput1, T2 & o_rOutput2 ) const; - template < typename T1, typename T2, typename T3 > bool InitGet( Result::Enum & o_rResult, T1 & o_rOutput1, T2 & o_rOutput2, T3 & o_rOutput3 ) const; - - bool IsEyeTrackerDevice(unsigned int i_DeviceID) const; - - bool HasData() const; - - void FetchNextFrame(); - void PreFetchThreadBody(); - - void BeginPreFetchThread(); - void EndPreFetchThread(); - - void CopyAndTransformT( const double i_Translation[ 3 ], double ( & io_Translation )[ 3 ] ) const; - void CopyAndTransformR( const double i_Rotation[ 9 ], double ( & io_Rotation )[ 9 ] ) const; - - ViconCGStreamType::UInt64 GetDevicePeriod( const unsigned int i_DeviceID ) const; - ViconCGStreamType::UInt64 GetDeviceStartTick( const unsigned int i_DeviceID ) const; - - // reference the client - std::shared_ptr< ViconCGStreamClientSDK::ICGClient > m_pClient; - std::string m_ServerName; - unsigned short m_ServerPort; - - std::shared_ptr< boost::thread > m_pPreFetchThread; - bool m_bContinuePreFetch; - - ViconCGStreamClientSDK::ICGFrameState m_LatestFrame; - ViconCGStreamClientSDK::ICGFrameState m_CachedFrame; - bool m_bNewCachedFrame; - - mutable boost::recursive_mutex m_FrameMutex; - - // What data is being requested - bool m_bSegmentDataEnabled; - bool m_bMarkerDataEnabled; - bool m_bUnlabeledMarkerDataEnabled; - bool m_bMarkerRayDataEnabled; - bool m_bDeviceDataEnabled; - bool m_bCentroidDataEnabled; - bool m_bGreyscaleDataEnabled; - bool m_bDebugDataEnabled; - bool m_bCameraWand2dDataEnabled; - bool m_bFrameRateInfoDataEnabled; - bool m_bVideoDataEnabled; - - // Axis mapping object - std::shared_ptr< VAxisMapping > m_pAxisMapping; - - // Current data filter - ViconCGStream::VFilter m_Filter; - - std::shared_ptr< VRetimingClient > m_pRetimingClient; - - unsigned int m_BufferSize; -}; - -/* Needs VC12 :( -template< typename T, typename... TArgs > -bool ViconDataStreamSDK::Core::VClient::InitGet( Result::Enum & o_rResult, T & o_rOutput TArgs &... o_rArgs ) const -{ - Clear( o_rOutput ); - return InitGet( o_rResult, o_rArgs ); -} -*/ - -template < typename T > -bool ViconDataStreamSDK::Core::VClient::InitGet( Result::Enum & o_rResult, T & o_rOutput ) const -{ - ClientUtils::Clear( o_rOutput ); - return InitGet( o_rResult ); -} - -template < typename T1, typename T2 > -bool ViconDataStreamSDK::Core::VClient::InitGet( Result::Enum & o_rResult, T1 & o_rOutput1, T2 & o_rOutput2 ) const -{ - ClientUtils::Clear( o_rOutput1 ); - ClientUtils::Clear( o_rOutput2 ); - return InitGet( o_rResult ); -} - -template < typename T1, typename T2, typename T3 > -bool ViconDataStreamSDK::Core::VClient::InitGet( Result::Enum & o_rResult, T1 & o_rOutput1, T2 & o_rOutput2, T3 & o_rOutput3 ) const -{ - ClientUtils::Clear( o_rOutput1 ); - ClientUtils::Clear( o_rOutput2 ); - ClientUtils::Clear( o_rOutput3 ); - return InitGet( o_rResult ); -} - -} // End of namespace Core -} // End of namespace ViconDataStreamSDK diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/RetimerUtils.cpp b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/RetimerUtils.cpp deleted file mode 100644 index 94ec52cab..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/RetimerUtils.cpp +++ /dev/null @@ -1,238 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#include "RetimerUtils.h" -#include "ClientUtils.h" - -#include - -namespace bmc = boost::math::constants; - -namespace ClientUtils -{ - typedef boost::array< double, 4 > Quaternion; - typedef boost::array< double, 3 > Axis; - typedef boost::array< double, 3 > Displacement; - - Quaternion Conjugate( const Quaternion & i_rInput ) - { - Quaternion conj = { -i_rInput[0], -i_rInput[1], -i_rInput[2], i_rInput[3] }; - return conj; - } - - Quaternion Inverse( const Quaternion & i_rInput ) - { - Quaternion inv = Conjugate( i_rInput ); - double DotProd = 0; - for( unsigned i = 0; i < 4; ++i ) DotProd += ( i_rInput[i] * i_rInput[i] ); - for( unsigned i = 0; i < 4; ++i ) inv[i] /= DotProd; - return inv; - } - - Axis Imaginary( const Quaternion & i_rInput ) - { - Axis axis = { i_rInput[0], i_rInput[1], i_rInput[2] }; - return axis; - } - - double Magnitude( const Axis & i_rInput ) - { - double mag = 0; - for( unsigned int i = 0; i < 3; ++i ) - { - mag += i_rInput[i] * i_rInput[i]; - } - return sqrt( mag ); - } - - - void Normalize( Axis & io_rInput ) - { - double Mag = Magnitude( io_rInput ); - io_rInput /= Mag; - } - - bool ToAxisAngle( const Quaternion & i_rInput, Axis & o_rAxis, double & o_rAngle ) - { - bool success = true; - - // angle is always positive - Axis imag = Imaginary( i_rInput ); - double mag = Magnitude( imag ); - o_rAngle = 2 * std::atan2( mag, i_rInput[3] ); - - // direc parallel to imag. part - o_rAxis = imag; - if( mag == 0.0 ) - { - // or signal exception here. - o_rAxis[2] = 1.0; - success = false; - } - else - { - // normalize direction vector - for( unsigned int i = 0; i < 3; ++i ) o_rAxis[i] /= mag; - } - return success; - } - - Quaternion FromAxisAngle( const Axis & i_rAxis, const double & i_rAngle ) - { - // This should take a normalized vector - Axis AxisCopy = i_rAxis; - Normalize( AxisCopy ); - - Quaternion output; - - // half angle - double a = i_rAngle * 0.5; - double s = std::sin( a ); - - for( int i = 0; i < 3; i++ ) - { - // imaginary vector is sine of half angle multiplied with axis - output[i] = s * AxisCopy[i]; - } - - // real part is cosine of half angle - output[3] = std::cos( a ); - - return output; - } - - RotationMatrix ToRotationMatrix( const Quaternion & i_rInput ) - { - double x2 = i_rInput[0] * i_rInput[0]; - double xy = i_rInput[0] * i_rInput[1]; - double rx = i_rInput[3] * i_rInput[0]; - double y2 = i_rInput[1] * i_rInput[1]; - double yz = i_rInput[1] * i_rInput[2]; - double ry = i_rInput[3] * i_rInput[1]; - double z2 = i_rInput[2] * i_rInput[2]; - double zx = i_rInput[2] * i_rInput[0]; - double rz = i_rInput[3] * i_rInput[2]; - double r2 = i_rInput[3] * i_rInput[3]; - - RotationMatrix Output; - // fill diagonal terms - Output[ 0 ] = r2 + x2 - y2 - z2; //(0,0) - Output[ 4 ] = r2 - x2 + y2 - z2; // (1,1) - Output[ 8 ] = r2 - x2 - y2 + z2; // (2,2) - - // fill off diagonal terms (output not transposed; differing from vnl_quaternion::rotation_matrix_transposed - Output[ 3 ] = 2 * ( xy + rz ); // (1,0) - Output[ 6 ] = 2 * ( zx - ry ); // (2,0) - Output[ 7 ] = 2 * ( yz + rx ); // (2,1) - Output[ 1 ] = 2 * ( xy - rz ); // (0,1) - Output[ 2 ] = 2 * ( zx + ry ); // (0,2) - Output[ 5 ] = 2 * ( yz - rx ); // (1,2) - - return Output; - } - - Axis operator*( const Axis & i_rLeft, double i_rVal ) - { - Axis Result; - std::transform( i_rLeft.begin(), i_rLeft.end(), Result.begin(), [&]( double x ){ return x * i_rVal; } ); - return Result; - } - - Axis operator+=( Axis & i_rLeft, const Axis & i_rRight ) - { - std::transform( i_rLeft.begin(), i_rLeft.end(), i_rRight.begin(), i_rLeft.begin(), []( double x, double y ){ return x + y; } ); - return i_rLeft; - } - - Quaternion operator*( const Quaternion & i_rLeft, const Quaternion & i_rRight ) - { - Quaternion output; - double r1 = i_rLeft[3]; // real and img parts of args - double r2 = i_rRight[3]; - Axis i1 = Imaginary( i_rLeft ); - Axis i2 = Imaginary( i_rRight ); - double real_v = ( r1 * r2 ) - DotProduct( i1, i2 ); // real&img of product q1*q2 - Axis img = CrossProduct( i1, i2 ); - Axis i2r1 = i2*r1; - Axis i1r2 = i1*r2; - Axis i2r1i1r2 = i2r1 + i1r2; - img += i2r1i1r2; - output = { img[0], img[1], img[2], real_v }; - return output; - } - - // Simple interpolation using linear interpolation and prediction - /// Returns a rotation prediction at time t3 from 2 rotation r1 at time t1 and r2 and time t2 - /// where t3 > t2 > t1 - // From http://answers.unity3d.com/questions/168779/extrapolating-quaternion-rotation.html - Quaternion PredictRotation( Quaternion r1, double t1, Quaternion r2, double t2, double t3 ) - { - Quaternion rot = r2 * Inverse( r1 ); // rot is rotation from t1 to t2 - - double dt = ( t3 - t1 ) / ( t2 - t1 ); // dt = extrapolation factor - - // Convert to Axis/Angle - Axis axis; - double angle; - if( !ToAxisAngle( rot, axis, angle ) ) - { - // Check the magnitude of the imaginary part is not zero, as then - // the axis of the quaternion will not be well defined. - return r1; - } - - // Assume shortest path - if( angle > bmc::pi< double >() ) angle -= ( 2 * bmc::pi< double >() ); - - // Multiple angle by extrapolation factor - angle = fmod( angle * dt, ( 2 * bmc::pi< double >() ) ); - - // Combine with first rotation - Quaternion r3 = FromAxisAngle( axis, angle ) * r1; - - return r3; - } - - /// Returns a translation prediction at time t3 from 2 translations at time d1, t1; d2, t2 where t3 > t2 > t1 - /// Can also be used for linear interpolation where t2 > t3 > t1 - Displacement PredictDisplacement( const Displacement & d1, double t1, const Displacement & d2, double t2, double t3 ) - { - - // disp is displacement from t1 to t2 - Displacement disp( d2 - d1 ); - - // extrapolation factor - double dt = ( t3 - t1 ) / ( t2 - t1 ); - - // multiply displacement by extrapolation factor - disp *= dt; - - // Combine with first displacement - Displacement d3 = d1 + disp; - - return d3; - } -} - - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/RetimerUtils.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/RetimerUtils.h deleted file mode 100644 index 03c1868e1..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/RetimerUtils.h +++ /dev/null @@ -1,61 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -#include - -namespace ClientUtils -{ - typedef boost::array< double, 4 > Quaternion; - typedef boost::array< double, 9 > RotationMatrix; - typedef boost::array< double, 3 > Axis; - typedef boost::array< double, 3 > Displacement; - - // Simple Quaternion and Vector operation, to remove vnl/tmv dependency - Quaternion Conjugate( const Quaternion & i_rInput ); - Quaternion Inverse( const Quaternion & i_rInput ); - - Axis Imaginary( const Quaternion & i_rInput ); - double Magnitude( const Axis & i_rInput ); - void Normalize( Axis & io_rInput ); - - bool ToAxisAngle( const Quaternion & i_rInput, Axis & o_rAxis, double & o_rAngle ); - Quaternion FromAxisAngle( const Axis & i_rAxis, const double & i_rAngle ); - RotationMatrix ToRotationMatrix( const Quaternion & i_rInput ); - - Axis operator*( const Axis & i_rLeft, double i_rVal ); - Axis operator+=( Axis & i_rLeft, const Axis & i_rRight ); - Quaternion operator*( const Quaternion & i_rLeft, const Quaternion & i_rRight ); - - /// Returns a rotation prediction at time t3 from 2 rotation r1 at time t1 and r2 and time t2 - /// where t3 > t2 > t1 - Quaternion PredictRotation( Quaternion r1, double t1, Quaternion r2, double t2, double t3 ); - - /// Returns a translation prediction at time t3 from 2 translations at time d1, t1; d2, t2 where t3 > t2 > t1 - /// Can also be used for linear interpolation where t2 > t3 > t1 - Displacement PredictDisplacement( const Displacement & d1, double t1, const Displacement & d2, double t2, double t3 ); -} - - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/RetimingClient.cpp b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/RetimingClient.cpp deleted file mode 100644 index ed0e63c6a..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/RetimingClient.cpp +++ /dev/null @@ -1,1129 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#include "RetimingClient.h" -#include "CoreClient.h" - -#include "RetimerUtils.h" - -#pragma warning ( push ) -#pragma warning ( disable : 4265 ) -#include -#pragma warning ( pop ) -#include - -#include -#include - -using namespace ClientUtils; - - - -namespace ViconDataStreamSDK -{ - - namespace Core - { - Result::Enum Adapt(const VSubjectPose::EResult & i_rResult) - { - switch (i_rResult) - { - case VSubjectPose::ESuccess: return Result::Success; - case VSubjectPose::ENoData: return Result::NoFrame; - case VSubjectPose::ENotConnected: return Result::NotConnected; - case VSubjectPose::EUnknownSubject: return Result::InvalidSubjectName; - case VSubjectPose::EEarly: return Result::EarlyDataRequested; - case VSubjectPose::ELate: return Result::LateDataRequested; - case VSubjectPose::EInvalid: return Result::InvalidOperation; - }; - return Result::Unknown; - } - - static unsigned int s_BufSize = 3; - - VRetimingClient::VRetimingClient(VClient & i_rClient) - : m_rClient(i_rClient) - , m_bInputStopped(false) - , m_bOutputStopped(false) - , m_OutputLatency(0.0) - , m_MaxPredictionTime(100) - , m_NetworkLatency( 0.3 ) - { - - } - - VRetimingClient::~VRetimingClient() - { - Disconnect(); - } - - // Connect client to the Vicon Data Stream - Result::Enum VRetimingClient::Connect( - std::shared_ptr< ViconCGStreamClientSDK::ICGClient > i_pClient, - const std::string & i_rHostName) - { - - Result::Enum Result = m_rClient.Connect(i_pClient, i_rHostName); - - if (Result == Result::Success) - { - // Ensure that segments are enabled - m_rClient.EnableSegmentData(); - - // We need to work in server push mode - m_rClient.SetStreamMode(StreamMode::ServerPush); - - // Start frame acquisition thread - m_pInputThread.reset(new boost::thread(boost::bind(&VRetimingClient::InputThread, this))); - } - - return Result; - } - - // Disconnect from the Vicon Data Stream - Result::Enum VRetimingClient::Disconnect() - { - // Stop output thread - StopOutput(); - - // Stop input thread - StopInput(); - - return m_rClient.Disconnect(); - } - - - Result::Enum VRetimingClient::StartOutput(double i_FrameRate) - { - // Can't start if our input thread isn't running - if (!m_pInputThread) - { - return Result::NotConnected; - } - - StopOutput(); - - // Run for a bit first - unsigned int FrameCount = 0; - unsigned int RunInPeriod = 20; - while (FrameCount < RunInPeriod) - { - m_rClient.GetFrame(); - ++FrameCount; - } - - - double FrameRate = 0; - - // Set output rate to input rate if none specified - if (i_FrameRate <= 0) - { - FrameRate = i_FrameRate; - } - else - { - m_rClient.GetFrameRate(FrameRate); - } - - - boost::recursive_mutex::scoped_lock Lock(m_FrameRateMutex); - m_FrameRate = FrameRate; - m_OutputFrameNumber = 0; - - // start thread - m_pOutputThread.reset(new boost::thread(boost::bind(&VRetimingClient::OutputThread, this))); - - return Result::Success; - - } - - Result::Enum VRetimingClient::StopOutput() - { - // stop thread - if (m_pOutputThread) - { - m_bOutputStopped = true; - m_pOutputThread->join(); - m_pOutputThread.reset(); - } - - m_OutputWait.notify_all(); - - return Result::Success; - } - - void VRetimingClient::StopInput() - { - m_bInputStopped = true; - - // Wait for it to stop - if (m_pInputThread) - { - m_pInputThread->join(); - m_pInputThread.reset(); - } - } - - bool VRetimingClient::IsRunning() const - { - return m_pOutputThread != nullptr; - } - - - bool VRetimingClient::InitGet(Result::Enum & o_rResult) const - { - o_rResult = Result::Success; - - if (!m_rClient.IsConnected()) - { - o_rResult = Result::NotConnected; - } - else if (m_Data.empty()) - { - o_rResult = Result::NoFrame; - } - - return (o_rResult == Result::Success); - } - - Result::Enum VRetimingClient::GetSubjectCount(unsigned int & o_rSubjectCount) const - { - boost::recursive_mutex::scoped_lock Lock(m_DataMutex); - - Result::Enum GetResult = Result::Success; - if ( InitGet( GetResult, o_rSubjectCount ) ) - { - o_rSubjectCount = static_cast(m_Data.size()); - } - - return GetResult; - } - - Result::Enum VRetimingClient::GetSubjectName(const unsigned int i_SubjectIndex, std::string& o_rSubjectName) const - { - boost::recursive_mutex::scoped_lock Lock(m_DataMutex); - - Result::Enum GetResult = Result::Success; - if (!InitGet(GetResult, o_rSubjectName)) - { - return GetResult; - } - - if (i_SubjectIndex >= m_Data.size()) - { - return Result::InvalidIndex; - } - - auto rPair = m_Data.begin(); - std::advance(rPair, i_SubjectIndex); - o_rSubjectName = rPair->first; - return Result::Success; - } - - Result::Enum VRetimingClient::GetSubjectRootSegmentName(const std::string & i_rSubjectName, std::string & o_rSegmentName) const - { - boost::recursive_mutex::scoped_lock Lock(m_DataMutex); - Clear(o_rSegmentName); - - std::shared_ptr< const VSubjectPose > pSubject; - Result::Enum Result = GetSubject(i_rSubjectName, pSubject ); - - if( Result == Result::Success ) - { - o_rSegmentName = pSubject->RootSegment; - } - - return Result; - } - - Result::Enum VRetimingClient::GetSegmentCount(const std::string& i_rSubjectName, unsigned int& o_rSegmentCount) const - { - boost::recursive_mutex::scoped_lock Lock(m_DataMutex); - Clear(o_rSegmentCount); - - std::shared_ptr< const VSubjectPose > pSubject; - Result::Enum Result = GetSubject(i_rSubjectName, pSubject); - - if( Result == Result::Success ) - { - o_rSegmentCount = static_cast< unsigned int >( pSubject->m_Segments.size() ); - } - - return Result; - } - - Result::Enum VRetimingClient::GetSegmentName(const std::string& i_rSubjectName, const unsigned int i_SegmentIndex, std::string& o_rSegmentName) const - { - boost::recursive_mutex::scoped_lock Lock(m_DataMutex); - Clear(o_rSegmentName); - - std::shared_ptr< const VSubjectPose > pSubject; - Result::Enum Result = GetSubject(i_rSubjectName, pSubject); - - if( Result == Result::Success ) - { - if( i_SegmentIndex < pSubject->m_Segments.size() ) - { - auto rPair = pSubject->m_Segments.begin(); - std::advance(rPair, i_SegmentIndex); - o_rSegmentName = rPair->first; - } - else - { - Result = Result::InvalidIndex; - } - } - - return Result; - } - - Result::Enum VRetimingClient::GetSegmentChildCount(const std::string& i_rSubjectName, const std::string& i_rSegmentName, unsigned int & o_rSegmentCount) const - { - boost::recursive_mutex::scoped_lock Lock(m_DataMutex); - Clear(o_rSegmentCount); - - std::shared_ptr< const VSubjectPose > pSubject; - Result::Enum Result = GetSubject(i_rSubjectName, pSubject); - - if( Result == Result::Success ) - { - const auto pSegment = pSubject->m_Segments.find(i_rSegmentName); - if( pSegment != pSubject->m_Segments.end() && pSegment->second ) - { - o_rSegmentCount = static_cast< unsigned int >( pSegment->second->m_Children.size() ); - } - else - { - Result = Result::InvalidSegmentName; - } - } - - return Result; - } - - Result::Enum VRetimingClient::GetSegmentChildName(const std::string& i_rSubjectName, const std::string& i_rSegmentName, unsigned int i_SegmentIndex, std::string& o_rSegmentName) const - { - boost::recursive_mutex::scoped_lock Lock(m_DataMutex); - Clear(o_rSegmentName); - - std::shared_ptr< const VSubjectPose > pSubject; - Result::Enum Result = GetSubject(i_rSubjectName, pSubject); - - if( Result == Result::Success ) - { - const auto pSegment = pSubject->m_Segments.find(i_rSegmentName); - if( pSegment != pSubject->m_Segments.end() && pSegment->second ) - { - if( i_SegmentIndex < pSegment->second->m_Children.size() ) - { - o_rSegmentName = pSegment->second->m_Children[i_SegmentIndex]; - } - else - { - Result = Result::InvalidIndex; - } - } - else - { - Result = Result::InvalidSegmentName; - } - } - - return Result; - } - - Result::Enum VRetimingClient::GetSegmentParentName(const std::string& i_rSubjectName, const std::string& i_rSegmentName, std::string& o_rSegmentName) const - { - boost::recursive_mutex::scoped_lock Lock(m_DataMutex); - Clear(o_rSegmentName); - - std::shared_ptr< const VSubjectPose > pSubject; - Result::Enum Result = GetSubject(i_rSubjectName, pSubject); - - if( Result == Result::Success ) - { - const auto pSegment = pSubject->m_Segments.find(i_rSegmentName); - if( pSegment != pSubject->m_Segments.end() && pSegment->second ) - { - o_rSegmentName = pSegment->second->Parent; - } - else - { - Result = Result::InvalidSegmentName; - } - } - - return Result; - } - - Result::Enum VRetimingClient::GetSegmentStaticTranslation(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double(&o_rThreeVector)[3]) const - { - boost::recursive_mutex::scoped_lock Lock(m_DataMutex); - - Result::Enum GetResult = Result::Success; - if( !InitGet(GetResult, o_rThreeVector) ) - { - return GetResult; - } - - std::shared_ptr< const VSubjectPose > pSubject; - GetResult = GetSubject(i_rSubjectName, pSubject); - - if( GetResult == Result::Success ) - { - // Get the segment - auto SegIt = pSubject->m_Segments.find(i_rSegmentName); - if( SegIt != pSubject->m_Segments.end() ) - { - std::copy(SegIt->second->T_Stat.begin(), SegIt->second->T_Stat.end(), o_rThreeVector); - } - } - return GetResult; - } - - Result::Enum VRetimingClient::GetSegmentStaticRotationHelical(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double(&o_rThreeVector)[3]) const - { - boost::recursive_mutex::scoped_lock Lock(m_DataMutex); - - Clear(o_rThreeVector); - // InitGet is called by GetSegmentStaticRotationMatrix. - - // Get the answer as a rotation matrix - double RotationArray[9]; - const Result::Enum _Result = GetSegmentStaticRotationMatrix(i_rSubjectName, i_rSegmentName, RotationArray); - - if( Result::Success == _Result ) - { - MatrixToHelical(RotationArray, o_rThreeVector); - } - - return _Result; - } - - Result::Enum VRetimingClient::GetSegmentStaticRotationMatrix(const std::string & i_rSubjectName, const std::string & i_rSegmentName, double(&o_rRotation)[9]) const - { - boost::recursive_mutex::scoped_lock Lock(m_DataMutex); - - Result::Enum GetResult = Result::Success; - if( !InitGet(GetResult, o_rRotation) ) - { - return GetResult; - } - - std::shared_ptr< const VSubjectPose > pSubject; - GetResult = GetSubject(i_rSubjectName, pSubject); - - if( GetResult == Result::Success ) - { - // Get the segment - auto SegIt = pSubject->m_Segments.find(i_rSegmentName); - if( SegIt != pSubject->m_Segments.end() ) - { - boost::array< double, 9 > StaticRotation = ClientUtils::ToRotationMatrix(SegIt->second->R_Stat); - std::copy(StaticRotation.begin(), StaticRotation.end(), o_rRotation); - } - } - return GetResult; - } - - Result::Enum VRetimingClient::GetSegmentStaticRotationQuaternion(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double(&o_rFourVector)[4]) const - { - boost::recursive_mutex::scoped_lock Lock(m_DataMutex); - - Result::Enum GetResult = Result::Success; - if( !InitGet(GetResult, o_rFourVector) ) - { - return GetResult; - } - - std::shared_ptr< const VSubjectPose > pSubject; - GetResult = GetSubject(i_rSubjectName, pSubject); - - if( GetResult == Result::Success ) - { - // Get the segment - auto SegIt = pSubject->m_Segments.find(i_rSegmentName); - if( SegIt != pSubject->m_Segments.end() ) - { - std::copy(SegIt->second->R_Stat.begin(), SegIt->second->R_Stat.end(), o_rFourVector); - } - } - - return GetResult; - } - - Result::Enum VRetimingClient::GetSegmentStaticRotationEulerXYZ(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double(&o_rThreeVector)[3]) const - { - boost::recursive_mutex::scoped_lock Lock(m_DataMutex); - - Clear(o_rThreeVector); - // InitGet is called by GetSegmentStaticRotationMatrix. - - // Get the answer as a rotation matrix - double RotationArray[9]; - const Result::Enum _Result = GetSegmentStaticRotationMatrix(i_rSubjectName, i_rSegmentName, RotationArray); - - if( Result::Success == _Result) - { - MatrixToEulerXYZ(RotationArray, o_rThreeVector); - } - - return _Result; - } - - - Result::Enum VRetimingClient::GetSegmentGlobalTranslation(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double(&o_rThreeVector)[3], bool& o_rbOccluded) const - { - boost::recursive_mutex::scoped_lock Lock(m_DataMutex); - - Result::Enum GetResult = Result::Success; - if (!InitGet(GetResult, o_rThreeVector, o_rbOccluded)) - { - return GetResult; - } - - std::shared_ptr< const VSubjectPose > pSubject; - GetResult = GetSubject(i_rSubjectName, pSubject ); - - if (GetResult == Result::Success) - { - // Get the segment - auto SegIt = pSubject->m_Segments.find(i_rSegmentName); - if (SegIt != pSubject->m_Segments.end() ) - { - std::copy(SegIt->second->T.begin(), SegIt->second->T.end(), o_rThreeVector); - o_rbOccluded = SegIt->second->bOccluded; - } - } - return GetResult; - } - - Result::Enum VRetimingClient::GetSegmentGlobalRotationHelical(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double(&o_rThreeVector)[3], bool& o_rbOccluded) const - { - boost::recursive_mutex::scoped_lock Lock(m_DataMutex); - - Clear(o_rThreeVector); - Clear(o_rbOccluded); - // InitGet is called by GetSegmentGlobalRotationMatrix. - - // Get the answer as a rotation matrix - double RotationArray[9]; - const Result::Enum _Result = GetSegmentGlobalRotationMatrix(i_rSubjectName, i_rSegmentName, RotationArray, o_rbOccluded); - - if (Result::Success == _Result && !o_rbOccluded) - { - MatrixToHelical(RotationArray, o_rThreeVector); - } - - return _Result; - } - - Result::Enum VRetimingClient::GetSegmentGlobalRotationMatrix(const std::string & i_rSubjectName, const std::string & i_rSegmentName, double(&o_rRotation)[9], bool & o_rbOccluded) const - { - boost::recursive_mutex::scoped_lock Lock(m_DataMutex); - - Result::Enum GetResult = Result::Success; - if (!InitGet(GetResult, o_rRotation, o_rbOccluded)) - { - return GetResult; - } - - std::shared_ptr< const VSubjectPose > pSubject; - GetResult = GetSubject(i_rSubjectName, pSubject); - - if (GetResult == Result::Success) - { - // Get the segment - auto SegIt = pSubject->m_Segments.find(i_rSegmentName); - if (SegIt != pSubject->m_Segments.end()) - { - boost::array< double, 9 > GlobalRotation = ClientUtils::ToRotationMatrix(SegIt->second->R); - std::copy(GlobalRotation.begin(), GlobalRotation.end(), o_rRotation); - o_rbOccluded = SegIt->second->bOccluded; - } - } - return GetResult; - } - - Result::Enum VRetimingClient::GetSegmentGlobalRotationQuaternion(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double(&o_rFourVector)[4], bool& o_rbOccluded) const - { - boost::recursive_mutex::scoped_lock Lock(m_DataMutex); - - Result::Enum GetResult = Result::Success; - if (!InitGet(GetResult, o_rFourVector, o_rbOccluded)) - { - return GetResult; - } - - std::shared_ptr< const VSubjectPose > pSubject; - GetResult = GetSubject(i_rSubjectName, pSubject); - - if (GetResult == Result::Success) - { - // Get the segment - auto SegIt = pSubject->m_Segments.find(i_rSegmentName); - if (SegIt != pSubject->m_Segments.end()) - { - std::copy(SegIt->second->R.begin(), SegIt->second->R.end(), o_rFourVector); - o_rbOccluded = SegIt->second->bOccluded; - } - } - return GetResult; - } - - Result::Enum VRetimingClient::GetSegmentGlobalRotationEulerXYZ(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double(&o_rThreeVector)[3], bool& o_rbOccluded) const - { - boost::recursive_mutex::scoped_lock Lock(m_DataMutex); - - Clear(o_rThreeVector); - Clear(o_rbOccluded); - // InitGet is called by GetSegmentGlobalRotationMatrix. - - // Get the answer as a rotation matrix - double RotationArray[9]; - const Result::Enum _Result = GetSegmentGlobalRotationMatrix(i_rSubjectName, i_rSegmentName, RotationArray, o_rbOccluded); - - if (Result::Success == _Result && !o_rbOccluded) - { - MatrixToEulerXYZ(RotationArray, o_rThreeVector); - } - - return _Result; - } - - Result::Enum VRetimingClient::GetSegmentLocalTranslation(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double(&o_rThreeVector)[3], bool& o_rbOccluded) const - { - Result::Enum GetResult = Result::Success; - if (!InitGet(GetResult, o_rThreeVector, o_rbOccluded)) - { - return GetResult; - } - - std::shared_ptr< const VSubjectPose > pSubject; - GetResult = GetSubject(i_rSubjectName, pSubject); - - if (GetResult == Result::Success) - { - // Get the segment - auto SegIt = pSubject->m_Segments.find(i_rSegmentName); - if (SegIt != pSubject->m_Segments.end()) - { - std::copy(SegIt->second->T_Rel.begin(), SegIt->second->T_Rel.end(), o_rThreeVector); - o_rbOccluded = SegIt->second->bOccluded; - } - } - return GetResult; - } - - Result::Enum VRetimingClient::GetSegmentLocalRotationHelical(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double(&o_rThreeVector)[3], bool& o_rbOccluded) const - { - boost::recursive_mutex::scoped_lock Lock(m_DataMutex); - - Clear(o_rThreeVector); - Clear(o_rbOccluded); - - // Get the answer as a rotation matrix - double RotationArray[9]; - const Result::Enum _Result = GetSegmentLocalRotationMatrix(i_rSubjectName, i_rSegmentName, RotationArray, o_rbOccluded); - - if (Result::Success == _Result && !o_rbOccluded) - { - MatrixToHelical(RotationArray, o_rThreeVector); - } - - return _Result; - } - - Result::Enum VRetimingClient::GetSegmentLocalRotationMatrix(const std::string & i_rSubjectName, const std::string & i_rSegmentName, double(&o_rRotation)[9], bool & o_rbOccluded) const - { - Result::Enum GetResult = Result::Success; - if (!InitGet(GetResult, o_rRotation, o_rbOccluded)) - { - return GetResult; - } - - std::shared_ptr< const VSubjectPose > pSubject; - GetResult = GetSubject(i_rSubjectName, pSubject); - - if (GetResult == Result::Success) - { - // Get the segment - auto SegIt = pSubject->m_Segments.find(i_rSegmentName); - if (SegIt != pSubject->m_Segments.end()) - { - boost::array< double, 9 > LocalRotation = ClientUtils::ToRotationMatrix(SegIt->second->R_Rel); - std::copy(LocalRotation.begin(), LocalRotation.end(), o_rRotation); - o_rbOccluded = SegIt->second->bOccluded; - } - } - return GetResult; - } - - Result::Enum VRetimingClient::GetSegmentLocalRotationQuaternion(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double(&o_rFourVector)[4], bool& o_rbOccluded) const - { - Result::Enum GetResult = Result::Success; - if (!InitGet(GetResult, o_rFourVector, o_rbOccluded)) - { - return GetResult; - } - - std::shared_ptr< const VSubjectPose > pSubject; - GetResult = GetSubject(i_rSubjectName, pSubject); - - if (GetResult == Result::Success) - { - // Get the segment - auto SegIt = pSubject->m_Segments.find(i_rSegmentName); - if (SegIt != pSubject->m_Segments.end()) - { - std::copy(SegIt->second->R_Rel.begin(), SegIt->second->R_Rel.end(), o_rFourVector); - o_rbOccluded = SegIt->second->bOccluded; - } - } - return GetResult; - } - - Result::Enum VRetimingClient::GetSegmentLocalRotationEulerXYZ(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double(&o_rThreeVector)[3], bool& o_rbOccluded) const - { - boost::recursive_mutex::scoped_lock Lock(m_DataMutex); - - Clear(o_rThreeVector); - Clear(o_rbOccluded); - - // Get the answer as a rotation matrix - double RotationArray[9]; - const Result::Enum _Result = GetSegmentLocalRotationMatrix(i_rSubjectName, i_rSegmentName, RotationArray, o_rbOccluded); - - if (Result::Success == _Result && !o_rbOccluded) - { - MatrixToEulerXYZ(RotationArray, o_rThreeVector); - } - - return _Result; - } - - Result::Enum VRetimingClient::GetSubject(const std::string & i_rSubjectName, std::shared_ptr< const VSubjectPose > & o_rpSubject ) const - { - boost::recursive_mutex::scoped_lock Lock(m_DataMutex); - - Result::Enum OutputResult = Result::InvalidSubjectName; - - auto DataIt = m_LatestOutputPoses.find(i_rSubjectName); - if (DataIt != m_LatestOutputPoses.end()) - { - o_rpSubject = DataIt->second; - if (!DataIt->second) - { - OutputResult = Result::NoFrame; - } - else - { - switch (DataIt->second->Result) - { - case Core::VSubjectPose::ESuccess: - OutputResult = Result::Success; - break; - default: - case Core::VSubjectPose::ENoData: - OutputResult = Result::NoFrame; - break; - case Core::VSubjectPose::EUnknownSubject: - OutputResult = Result::InvalidSubjectName; - break; - case Core::VSubjectPose::EEarly: - OutputResult = Result::EarlyDataRequested; - break; - case Core::VSubjectPose::ELate: - OutputResult = Result::LateDataRequested; - break; - } - } - } - - return OutputResult; - } - - - Result::Enum VRetimingClient::WaitForFrame() const - { - boost::recursive_mutex::scoped_lock Lock(m_DataMutex); - - if (!IsRunning()) - { - return Result::NoFrame; - } - - m_OutputWait.wait(Lock); - return Result::Success; - } - - void VRetimingClient::SetOutputLatency(double i_OutputLatency) - { - boost::recursive_mutex::scoped_lock Lock(m_DataMutex); - m_OutputLatency = i_OutputLatency; - } - - double VRetimingClient::OutputLatency() const - { - boost::recursive_mutex::scoped_lock Lock(m_DataMutex); - return m_OutputLatency; - } - - void VRetimingClient::SetMaximumPrediction(double i_MaxPrediction) - { - boost::recursive_mutex::scoped_lock Lock(m_DataMutex); - m_MaxPredictionTime = i_MaxPrediction; - } - - double VRetimingClient::MaximumPrediction() const - { - boost::recursive_mutex::scoped_lock Lock(m_DataMutex); - return m_MaxPredictionTime; - } - - void VRetimingClient::AddData(const std::string & i_rObjectName, std::shared_ptr< VSubjectPose > i_pData) - { - boost::recursive_mutex::scoped_lock Lock(m_DataMutex); - - // DSSDK-210. Don't add occluded data, as we will be able to interpolate from good data - // as long as gaps are not too long. If an object becomes occluded for a significant time, - // Predict will return ELate when the data becomes too stale. - bool bOccluded = false; - // For multi-segment subjects, don't add if any segments are occluded. - for (const auto & rSegment : i_pData->m_Segments) - { - if (rSegment.second->bOccluded) - { - bOccluded = true; - //break; - } - } - - if (bOccluded) - { - return; - } - - auto & rData = m_Data[i_rObjectName]; - - // Stamp the receipt time as current time now less the system latency - i_pData->ReceiptTime = static_cast< double >(m_Timer.elapsed().wall) / 1000000.0 - i_pData->Latency; - rData.push_back(i_pData); - - while (rData.size() > s_BufSize) - { - rData.pop_front(); - } - } - - - Result::Enum VRetimingClient::UpdateFrame(double i_rOffset) - { - //const auto Before = m_Timer.elapsed().wall; - if (IsRunning()) - { - return Result::InvalidOperation; - } - - boost::recursive_mutex::scoped_lock Lock(m_DataMutex); - - m_LatestOutputPoses.clear(); - - Result::Enum Result = Result::Success; - - for (const auto & Pair : m_Data) - { - std::shared_ptr< const VSubjectPose > pPose; - const auto & rData = Pair.second; - if (rData.size() >= 2) - { - double Now = static_cast< double >(m_Timer.elapsed().wall) / 1000000.0; - double PTime = Now + i_rOffset; - pPose = Predict(rData.front(), rData.back(), PTime); - if (pPose->Result != VSubjectPose::ESuccess) - { - Result = Adapt( pPose->Result ); - } - } - - m_LatestOutputPoses[Pair.first] = pPose; - } - - //const auto After = m_Timer.elapsed().wall; - //std::cout << "Update Frame took " << (After - Before) << " ns" << std::endl;; - return Result; - } - - void VRetimingClient::InputThread() - { - - while (!m_bInputStopped) - { - if (m_rClient.IsConnected()) - { - // Get a frame - while (m_rClient.GetFrame() != Result::Success && m_rClient.IsConnected()) - { - // Sleep a little so that we don't lumber the CPU with a busy poll -#ifdef WIN32 - //Sleep(200); -#else - sleep( 1 ); -#endif - } - - double Latency = 0; - Latency += m_NetworkLatency; - - double SystemLatency; - if (m_rClient.GetLatencyTotal(SystemLatency) == Result::Success) - { - Latency += SystemLatency * 1000.0; - } - - unsigned int FrameNumber; - double FrameRateHz; - Result::Enum FrameNumberResult = m_rClient.GetFrameNumber(FrameNumber); - Result::Enum FrameRateResult = m_rClient.GetFrameRate(FrameRateHz); - - if (FrameNumberResult == Result::Success && FrameRateResult == Result::Success) - { - // Get a lock here so that we add all subjects from one frame together. - boost::recursive_mutex::scoped_lock Lock(m_DataMutex); - - double FrameReceiptTime = (1.0 / FrameRateHz) * static_cast(FrameNumber); - - // Count the number of subjects - unsigned int SubjectCount; - m_rClient.GetSubjectCount(SubjectCount); - for (unsigned int SubjectIndex = 0; SubjectIndex < SubjectCount; ++SubjectIndex) - { - - std::shared_ptr< VSubjectPose > pPoseData( new VSubjectPose() ); - - pPoseData->FrameTime = FrameReceiptTime; - pPoseData->Latency = Latency; - - // Get the subject name - std::string SubjectName; - m_rClient.GetSubjectName(SubjectIndex, SubjectName); - - // Get the root segment - std::string RootSegment; - m_rClient.GetSubjectRootSegmentName(SubjectName, RootSegment); - - pPoseData->RootSegment = RootSegment; - - // Count the number of segments - unsigned int SegmentCount; - m_rClient.GetSegmentCount(SubjectName, SegmentCount); - for (unsigned int SegmentIndex = 0; SegmentIndex < SegmentCount; ++SegmentIndex) - { - std::string SegmentName; - m_rClient.GetSegmentName(SubjectName, SegmentIndex, SegmentName); - - std::shared_ptr< VSegmentPose > pSegmentPoseData( new VSegmentPose() ); - pSegmentPoseData->Name = SegmentName; - - // Get our parent name (if we have one) - std::string ParentName; - if (m_rClient.GetSegmentParentName(SubjectName, SegmentName, ParentName) == Result::Success) - { - pSegmentPoseData->Parent = ParentName; - } - - // Add some information about the children of this segment - unsigned int ChildSegmentCount; - if( m_rClient.GetSegmentChildCount(SubjectName, SegmentName, ChildSegmentCount) == Result::Success ) - { - for( unsigned int ChildSegmentIndex = 0; ChildSegmentIndex < ChildSegmentCount; ++ChildSegmentIndex ) - { - std::string ChildSegmentName; - if( m_rClient.GetSegmentChildName(SubjectName, SegmentName, ChildSegmentIndex, ChildSegmentName) == Result::Success ) - { - pSegmentPoseData->m_Children.push_back(ChildSegmentName); - } - } - } - - // Get the global segment translation - bool bOccluded = false; - double Translation[3]; - m_rClient.GetSegmentGlobalTranslation(SubjectName, SegmentName, Translation, bOccluded); - std::copy(Translation, Translation + 3, pSegmentPoseData->T.begin()); - - // Get the global segment rotation in quaternion co-ordinates - double Rotation[4]; - m_rClient.GetSegmentGlobalRotationQuaternion(SubjectName, SegmentName, Rotation, bOccluded); - std::copy(Rotation, Rotation + 4, pSegmentPoseData->R.begin()); - - // Get local translation - double LocalTranslation[3]; - m_rClient.GetSegmentLocalTranslation(SubjectName, SegmentName, LocalTranslation, bOccluded); - std::copy(LocalTranslation, LocalTranslation + 3, pSegmentPoseData->T_Rel.begin()); - - // And local rotation - double LocalRotation[4]; - m_rClient.GetSegmentLocalRotationQuaternion(SubjectName, SegmentName, LocalRotation, bOccluded); - std::copy(LocalRotation, LocalRotation + 4, pSegmentPoseData->R_Rel.begin()); - - // Get static translation - double StaticTranslation[3]; - m_rClient.GetSegmentStaticTranslation(SubjectName, SegmentName, StaticTranslation); - std::copy(StaticTranslation, StaticTranslation + 3, pSegmentPoseData->T_Stat.begin()); - - // And static rotation - double StaticRotation[4]; - m_rClient.GetSegmentStaticRotationQuaternion(SubjectName, SegmentName, StaticRotation); - std::copy(StaticRotation, StaticRotation + 4, pSegmentPoseData->R_Stat.begin()); - - pSegmentPoseData->bOccluded = bOccluded; - - pPoseData->m_Segments[ SegmentName ] = pSegmentPoseData; - } - - AddData(SubjectName, pPoseData); - } - } - } - } - - m_bInputStopped = false; - - } - - void VRetimingClient::OutputThread() - { - // Keep track of retimed output frame number - uint64_t CurrentOutputFrame = 0; - - while (!m_bOutputStopped) - { - // Calculate time for this frame - double ThisFrameTime; - { - boost::recursive_mutex::scoped_lock Lock(m_FrameRateMutex); - ++m_OutputFrameNumber; - ThisFrameTime = m_FrameRate * static_cast(m_OutputFrameNumber); - } - - { - boost::recursive_mutex::scoped_lock Lock(m_DataMutex); - - // Interpolate output positions if required. - double Now = static_cast< double >(m_Timer.elapsed().wall) / 1000000.0 - m_OutputLatency; - - for (auto Pair : m_Data) - { - if (Pair.second.size() >= 2) - { - m_LatestOutputPoses[Pair.first] = Predict(Pair.second.front(), Pair.second.back(), Now); - } - } - - // Output the frame - m_OutputWait.notify_all(); - } - - // Increment our output frame number - ++CurrentOutputFrame; - - // Yield until next frame is required. - //UtilsThreading::Sleep(static_cast((1.0 / m_FrameRate) * 1000)); - std::this_thread::sleep_for(std::chrono::milliseconds(static_cast((1.0 / m_FrameRate) * 1000))); - - } - - m_bOutputStopped = false; - } - - std::shared_ptr< const VSubjectPose > VRetimingClient::Predict(std::shared_ptr< const VSubjectPose > p1, std::shared_ptr< const VSubjectPose > p2, double t) const - { - std::shared_ptr< VSubjectPose > pOutput( new VSubjectPose() ); - - if (p2->ReceiptTime < p1->ReceiptTime) - { - pOutput->Result = VSubjectPose::EInvalid; - return pOutput; - } - - if (t < p1->ReceiptTime) - { - pOutput->Result = VSubjectPose::EEarly; - return pOutput; - } - - if (t > p2->ReceiptTime) - { - if (t - p2->ReceiptTime > m_MaxPredictionTime) - { - pOutput->Result = VSubjectPose::ELate; - return pOutput; - } - } - - pOutput->FrameTime = t; - pOutput->ReceiptTime = t; - pOutput->Result = VSubjectPose::ESuccess; - pOutput->RootSegment = p1->RootSegment; - pOutput->Name = p1->Name; - - for (const auto SegIt : p1->m_Segments) - { - // Get corresponding segment from p2-> - for (const auto SegIt2 : p2->m_Segments) - { - std::shared_ptr< VSegmentPose > pSegment = SegIt.second; - std::shared_ptr< VSegmentPose > pSegment2 = SegIt2.second; - - if (pSegment2->Name == pSegment->Name) - { - - std::shared_ptr< VSegmentPose > pOutputSegment( new VSegmentPose() ); - pOutputSegment->Name = pSegment->Name; - pOutputSegment->Parent = pSegment->Parent; - std::copy(pSegment->T_Stat.begin(), pSegment->T_Stat.end(), pOutputSegment->T_Stat.begin()); - std::copy(pSegment->R_Stat.begin(), pSegment->R_Stat.end(), pOutputSegment->R_Stat.begin()); - - pOutputSegment->m_Children.reserve(pSegment->m_Children.size()); - std::copy(pSegment->m_Children.begin(), pSegment->m_Children.end(), std::back_inserter( pOutputSegment->m_Children ) ); - - pOutputSegment->bOccluded = pSegment->bOccluded || pSegment2->bOccluded; - - pOutputSegment->T = ClientUtils::PredictDisplacement(pSegment->T, p1->ReceiptTime, pSegment2->T, p2->ReceiptTime, t); - pOutputSegment->R = ClientUtils::PredictRotation(pSegment->R, p1->ReceiptTime, pSegment2->R, p2->ReceiptTime, t); - pOutputSegment->T_Rel = ClientUtils::PredictDisplacement(pSegment->T_Rel, p1->ReceiptTime, pSegment2->T_Rel, p2->ReceiptTime, t); - pOutputSegment->R_Rel = ClientUtils::PredictRotation(pSegment->R_Rel, p1->ReceiptTime, pSegment2->R_Rel, p2->ReceiptTime, t); - - pOutput->m_Segments[ pSegment->Name ] = pOutputSegment; - - break; - } - } - } - - return pOutput; - } - } -} diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/RetimingClient.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/RetimingClient.h deleted file mode 100644 index 471fe3130..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/RetimingClient.h +++ /dev/null @@ -1,254 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "Constants.h" -#include "ClientUtils.h" - -namespace ViconCGStreamClientSDK -{ - class ICGClient; -} - -namespace ViconDataStreamSDK -{ - - namespace Core - { - class VSegmentPose - { - public: - - VSegmentPose() : - bOccluded(false) {} - - std::string Name; - std::string Parent; - - boost::array< double, 3 > T; - boost::array< double, 4 > R; - - boost::array< double, 3 > T_Rel; - boost::array< double, 4 > R_Rel; - - boost::array< double, 3 > T_Stat; - boost::array< double, 4 > R_Stat; - - std::vector< std::string > m_Children; - - bool bOccluded; - }; - - class VSubjectPose - { - public: - - enum EResult - { - ESuccess, - ENoData, - ENotConnected, - EUnknownSubject, - EEarly, - ELate, - EInvalid - }; - - VSubjectPose() - : Result(EInvalid) - , FrameTime(0) - , ReceiptTime(0) - {} - - EResult Result; - - std::string Name; - std::string RootSegment; - - double Latency; - double FrameTime; - double ReceiptTime; - - std::map< std::string, std::shared_ptr< VSegmentPose > > m_Segments; - }; - - class VClient; - - class VRetimingClient - { - public: - - VRetimingClient( VClient & i_rClient ); - ~VRetimingClient(); - - // Connect client to the Vicon Data Stream - Result::Enum Connect(std::shared_ptr< ViconCGStreamClientSDK::ICGClient > i_pClient, - const std::string & i_rHostName); - - // Disconnect from the Vicon Data Stream - Result::Enum Disconnect(); - - // Interface - Result::Enum StartOutput( double i_FrameRate ); - Result::Enum StopOutput(); - bool IsRunning() const; - - // Store a predicted pose for all subjects currently in the stream at the current time. - Result::Enum UpdateFrame(double i_Offset); - - Result::Enum GetSubjectCount(unsigned int & o_rSubjectCount) const; - Result::Enum GetSubjectName(const unsigned int i_SubjectIndex, std::string& o_rSubjectName) const; - Result::Enum GetSubjectRootSegmentName(const std::string & i_rSubjectName, std::string & o_rSegmentName) const; - - Result::Enum GetSegmentCount(const std::string& i_rSubjectName, unsigned int& o_rSegmentCount) const; - Result::Enum GetSegmentName(const std::string& i_rSubjectName, const unsigned int i_SegmentIndex, std::string& o_rSegmentName) const; - Result::Enum GetSegmentChildCount(const std::string& i_rSubjectName, const std::string& i_rSegmentName, unsigned int & o_rSegmentCount) const; - Result::Enum GetSegmentChildName(const std::string& i_rSubjectName, const std::string& i_rSegmentName, unsigned int i_SegmentIndex, std::string& o_rSegmentName) const; - Result::Enum GetSegmentParentName(const std::string& i_rSubjectName, const std::string& i_rSegmentName, std::string& o_rSegmentName) const; - - Result::Enum GetSegmentStaticTranslation(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double(&o_rThreeVector)[3]) const; - Result::Enum GetSegmentStaticRotationHelical(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double(&o_rThreeVector)[3]) const; - Result::Enum GetSegmentStaticRotationMatrix(const std::string & i_rSubjectName, const std::string & i_rSegmentName, double(&o_rRotation)[9]) const; - Result::Enum GetSegmentStaticRotationQuaternion(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double(&o_rFourVector)[4]) const; - Result::Enum GetSegmentStaticRotationEulerXYZ(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double(&o_rThreeVector)[3]) const; - - Result::Enum GetSegmentGlobalTranslation(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double(&o_rThreeVector)[3], bool& o_rbOccluded) const; - Result::Enum GetSegmentGlobalRotationHelical(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double(&o_rThreeVector)[3], bool& o_rbOccluded) const; - Result::Enum GetSegmentGlobalRotationMatrix(const std::string & i_rSubjectName, const std::string & i_rSegmentName, double(&o_rRotation)[9], bool & o_rbOccluded) const; - Result::Enum GetSegmentGlobalRotationQuaternion(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double(&o_rFourVector)[4], bool& o_rbOccluded) const; - Result::Enum GetSegmentGlobalRotationEulerXYZ(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double(&o_rThreeVector)[3], bool& o_rbOccluded) const; - - Result::Enum GetSegmentLocalTranslation(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double(&o_pThreeVector)[3], bool& o_rbOccluded) const; - Result::Enum GetSegmentLocalRotationHelical(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double(&o_rThreeVector)[3], bool& o_rbOccluded) const; - Result::Enum GetSegmentLocalRotationMatrix(const std::string & i_rSubjectName, const std::string & i_rSegmentName, double(&o_rRotation)[9], bool & o_rbOccluded) const; - Result::Enum GetSegmentLocalRotationQuaternion(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double(&o_rFourVector)[4], bool& o_rbOccluded) const; - Result::Enum GetSegmentLocalRotationEulerXYZ(const std::string& i_rSubjectName, const std::string& i_rSegmentName, double(&o_rThreeVector)[3], bool& o_rbOccluded) const; - - // Wait for a frame, populates pose of all objects in o_rSubjects mapped to name - Result::Enum WaitForFrame() const; - - // Set the latency required in the output stream; may be used to avoid the need to predict data forwards, - // where a small amount of latency is acceptable. - void SetOutputLatency( double i_OutputLatency ); - - // Return the output latency specified - double OutputLatency() const; - - // Set the maximum amount by which the interpolation engine will predict forward - void SetMaximumPrediction( double i_MaxPrediction ); - - // Return the maximum prediction used by the system - double MaximumPrediction() const; - - // Set an estimate for (fixed) network latency - void SetNetworkLatency(double i_Latency); - - // This needs to be public, or make the unit test a friend - std::shared_ptr< const VSubjectPose > Predict(std::shared_ptr< const VSubjectPose > p1, std::shared_ptr< const VSubjectPose > p2, double t) const; - - private: - - void InputThread(); - void StopInput(); - void AddData(const std::string & i_rName, std::shared_ptr< VSubjectPose > i_pData); - Result::Enum GetSubject(const std::string & i_rSubjectName, std::shared_ptr< const VSubjectPose > & o_rpSubject) const; - - - bool InitGet(Result::Enum & o_rResult) const; - template < typename T > bool InitGet(Result::Enum & o_rResult, T & o_rOutput) const; - template < typename T1, typename T2 > bool InitGet(Result::Enum & o_rResult, T1 & o_rOutput1, T2 & o_rOutput2) const; - template < typename T1, typename T2, typename T3 > bool InitGet(Result::Enum & o_rResult, T1 & o_rOutput1, T2 & o_rOutput2, T3 & o_rOutput3) const; - - private: - - - VClient & m_rClient; - - void OutputThread(); - - - mutable boost::recursive_mutex m_DataMutex; - std::map< std::string, std::deque< std::shared_ptr< const VSubjectPose > > > m_Data; - - boost::recursive_mutex m_FrameRateMutex; - double m_FrameRate; - unsigned int m_OutputFrameNumber; - - mutable boost::condition m_OutputWait; - std::map< std::string, std::shared_ptr< const VSubjectPose > > m_LatestOutputPoses; - - std::unique_ptr< boost::thread > m_pInputThread; - bool m_bInputStopped; - - std::unique_ptr< boost::thread > m_pOutputThread; - bool m_bOutputStopped; - - // Required output latency (in milliseconds) - double m_OutputLatency; - - // Maximum time we should predict forwards (in milliseconds) - double m_MaxPredictionTime; - - // Estimated amount for network latency - double m_NetworkLatency; - - boost::timer::cpu_timer m_Timer; - }; - - template < typename T > - bool ViconDataStreamSDK::Core::VRetimingClient::InitGet(Result::Enum & o_rResult, T & o_rOutput) const - { - ClientUtils::Clear(o_rOutput); - return InitGet(o_rResult); - } - - template < typename T1, typename T2 > - bool ViconDataStreamSDK::Core::VRetimingClient::InitGet(Result::Enum & o_rResult, T1 & o_rOutput1, T2 & o_rOutput2) const - { - ClientUtils::Clear(o_rOutput1); - ClientUtils::Clear(o_rOutput2); - return InitGet(o_rResult); - } - - template < typename T1, typename T2, typename T3 > - bool ViconDataStreamSDK::Core::VRetimingClient::InitGet(Result::Enum & o_rResult, T1 & o_rOutput1, T2 & o_rOutput2, T3 & o_rOutput3) const - { - ClientUtils::Clear(o_rOutput1); - ClientUtils::Clear(o_rOutput2); - ClientUtils::Clear(o_rOutput3); - return InitGet(o_rResult); - } - } -} diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/ViconDataStreamSDKCoreVersion.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/ViconDataStreamSDKCoreVersion.h deleted file mode 100644 index c3a1bb6a6..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/ViconDataStreamSDKCoreVersion.h +++ /dev/null @@ -1,27 +0,0 @@ -#define VICONDATASTREAMSDKCORE_VERSION_MAJOR 1 -#define VICONDATASTREAMSDKCORE_VERSION_MINOR 8 -#define VICONDATASTREAMSDKCORE_VERSION_POINT 0 -#define VICONDATASTREAMSDKCORE_VERSION_REVISION_CONTROL_SYSTEM "unknown" -#define VICONDATASTREAMSDKCORE_VERSION_REVISION 0 -#define VICONDATASTREAMSDKCORE_VERSION_MONOTONIC_REVISION 0 -#define VICONDATASTREAMSDKCORE_VERSION_PUBLIC_REVISION_STRING "0" -#define VICONDATASTREAMSDKCORE_VERSION_RELEASE "" -#define VICONDATASTREAMSDKCORE_VERSION_BRANCH "unknown" -#define VICONDATASTREAMSDKCORE_VERSION_REPOSITORY "unknown" -#define VICONDATASTREAMSDKCORE_VERSION_CHANGESET "0" -#define VICONDATASTREAMSDKCORE_VERSION_LOCAL_CHANGESET "0" -#define VICONDATASTREAMSDKCORE_VERSION_LOCAL_BRANCH "unknown" - -#define VICONDATASTREAMSDKCORE_FULL_VERSION_STRING "1.8.0.0" -#define VICONDATASTREAMSDKCORE_VERSION_STRING "1.8" -#define VICONDATASTREAMSDKCORE_COMPANY "Vicon Motion Systems Ltd" -#define VICONDATASTREAMSDKCORE_COPYRIGHT "Copyright \x00A9 2018 Vicon Motion Systems Ltd. All rights reserved." -#define VICONDATASTREAMSDKCORE_TRADEMARK "Vicon\x00AE is a registered trademark of OMG Plc." -#define VICONDATASTREAMSDKCORE_PROJECT_NAME "ViconDataStreamSDKCore" - -#define VICONDATASTREAMSDKCORE_FULL_VERSION_STRING_L L"1.8.0.0" -#define VICONDATASTREAMSDKCORE_VERSION_STRING_L L"1.8" -#define VICONDATASTREAMSDKCORE_COMPANY_L L"Vicon Motion Systems Ltd" -#define VICONDATASTREAMSDKCORE_COPYRIGHT_L L"Copyright \x00A9 2018 Vicon Motion Systems Ltd. All rights reserved." -#define VICONDATASTREAMSDKCORE_TRADEMARK_L L"Vicon\x00AE is a registered trademark of OMG Plc." -#define VICONDATASTREAMSDKCORE_PROJECT_NAME_L L"ViconDataStreamSDKCore" diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/makefile b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/makefile deleted file mode 100644 index b36dddcfa..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/makefile +++ /dev/null @@ -1,140 +0,0 @@ -# Makefile for ViconDataStreamSDKCore - -ifndef VERBOSE -.SILENT : -endif -.SUFFIXES : - -ifdef CONFIG -ifneq ($(CONFIG),Debug) -ifneq ($(CONFIG),InternalRelease) -ifneq ($(CONFIG),Release) -Error: unknown configuration. -endif -endif -endif -else -CONFIG=Debug -endif - -INCLUDEPATHS=../../../../thirdparty/Boost/boost-1.58.0-dynamic-linux-x64/installed/include . .. ../.. -INCLUDEPATHS_Debug=Debug -INCLUDEPATHS_InternalRelease=InternalRelease -INCLUDEPATHS_Release=Release -LIBRARYPATHS=../../../../thirdparty/Boost/boost-1.58.0-dynamic-linux-x64/installed/lib -LIBRARYPATHS_Debug=../../../../lib/Debug ../../../../bin/Debug -LIBRARYPATHS_InternalRelease=../../../../lib/InternalRelease ../../../../bin/InternalRelease -LIBRARYPATHS_Release=../../../../lib/Release ../../../../bin/Release -DEPENDENCIES=ViconCGStreamClientSDK ViconCGStreamClient ViconCGStream StreamCommon -LIBRARIES= -LIBRARIES_Debug=boost_wserialization-mt-d boost_wave-mt-d boost_unit_test_framework-mt-d boost_timer-mt-d boost_thread-mt-d boost_system-mt-d boost_signals-mt-d boost_serialization-mt-d boost_regex-mt-d boost_random-mt-d boost_python-mt-d boost_program_options-mt-d boost_prg_exec_monitor-mt-d boost_math_tr1l-mt-d boost_math_tr1f-mt-d boost_math_tr1-mt-d boost_math_c99l-mt-d boost_math_c99f-mt-d boost_math_c99-mt-d boost_log_setup-mt-d boost_log-mt-d boost_locale-mt-d boost_iostreams-mt-d boost_graph-mt-d boost_filesystem-mt-d boost_date_time-mt-d boost_coroutine-mt-d boost_context-mt-d boost_container-mt-d boost_chrono-mt-d boost_atomic-mt-d -LIBRARIES_InternalRelease=boost_wserialization-mt boost_wave-mt boost_unit_test_framework-mt boost_timer-mt boost_thread-mt boost_system-mt boost_signals-mt boost_serialization-mt boost_regex-mt boost_random-mt boost_python-mt boost_program_options-mt boost_prg_exec_monitor-mt boost_math_tr1l-mt boost_math_tr1f-mt boost_math_tr1-mt boost_math_c99l-mt boost_math_c99f-mt boost_math_c99-mt boost_log_setup-mt boost_log-mt boost_locale-mt boost_iostreams-mt boost_graph-mt boost_filesystem-mt boost_date_time-mt boost_coroutine-mt boost_context-mt boost_container-mt boost_chrono-mt boost_atomic-mt -LIBRARIES_Release=boost_wserialization-mt boost_wave-mt boost_unit_test_framework-mt boost_timer-mt boost_thread-mt boost_system-mt boost_signals-mt boost_serialization-mt boost_regex-mt boost_random-mt boost_python-mt boost_program_options-mt boost_prg_exec_monitor-mt boost_math_tr1l-mt boost_math_tr1f-mt boost_math_tr1-mt boost_math_c99l-mt boost_math_c99f-mt boost_math_c99-mt boost_log_setup-mt boost_log-mt boost_locale-mt boost_iostreams-mt boost_graph-mt boost_filesystem-mt boost_date_time-mt boost_coroutine-mt boost_context-mt boost_container-mt boost_chrono-mt boost_atomic-mt -DEFINES=TCM_LINUX TCM_UNIX PROJECT_SOURCE_PATH=\"/home/swtest/workspace/DatastreamSDK_Linux_64_Branch_Release/Binary/bin/Release/deployment/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore\" -DEFINES_Debug=_DEBUG -DEFINES_InternalRelease=VICON_INTERNAL_RELEASE NDEBUG -DEFINES_Release=NDEBUG TCM_OFF_SITE -ENV_CPU=x64 - -SOURCEDIRECTORY=../../../.. -PROJECTPATH=. -BINARYDIRECTORY=../../../.. -INTERMEDIATEDIRECTORY=. -LIBRARYDIRECTORY=../../../../lib -OUTPUTDIRECTORY=../../../../bin - -include $(BINARYDIRECTORY)/gcc.mk - -HIDE_BOOST_SCRIPT=hide_boost_version_script -ifneq ($(HIDE_BOOST),) - HIDE_BOOST_LD_PARAM= -Wl,--version-script=$(HIDE_BOOST_SCRIPT) - HIDE_BOOST_LD_PREREQ=$(HIDE_BOOST_SCRIPT) -endif -all: all_$(CONFIG) - -all_Debug: $(LIBRARYDIRECTORY)/$(CONFIG)/libViconDataStreamSDKCore.a -all_InternalRelease: $(LIBRARYDIRECTORY)/$(CONFIG)/libViconDataStreamSDKCore.a -all_Release: $(LIBRARYDIRECTORY)/$(CONFIG)/libViconDataStreamSDKCore.a - -OBJECTS=$(CONFIG)/ClientUtils.o $(CONFIG)/CoreClient.o $(CONFIG)/AxisMapping.o $(CONFIG)/RetimingClient.o $(CONFIG)/RetimerUtils.o - -CXXFLAGS=$(CXXFLAGS_$(CONFIG)) $(L_INCLUDEPATHS) $(L_INCLUDEPATHS_$(CONFIG)) $(L_INCLUDES) $(L_INCLUDES_$(CONFIG)) $(L_DEFINES) $(L_DEFINES_$(CONFIG)) -CCFLAGS=$(CCFLAGS_$(CONFIG)) $(L_INCLUDEPATHS) $(L_INCLUDEPATHS_$(CONFIG)) $(L_INCLUDES) $(L_INCLUDES_$(CONFIG)) $(L_DEFINES) $(L_DEFINES_$(CONFIG)) -LDFLAGS=$(LDFLAGS_$(CONFIG)) $(L_LIBRARYPATHS) $(L_LIBRARYPATHS_$(CONFIG)) -CXXFLAGS+=-Wno-unused-local-typedefs -CXXFLAGS+=-Wno-delete-non-virtual-dtor -std=c++14 -#Android toolchain does not include librt but integrates some of its functionality into Android libc. -ifndef ANDROID_TARGET_ARCH -LDFLAGS+=-lrt -endif - - -$(LIBRARYDIRECTORY)/Debug/libViconDataStreamSDKCore.a: makefile $(OBJECTS) - @echo \[1\;35mLinking archive library $@\ - @mkdir -p $(@D) - $(AR) rcs $@ $(OBJECTS) - -$(LIBRARYDIRECTORY)/InternalRelease/libViconDataStreamSDKCore.a: makefile $(OBJECTS) - @echo \[1\;35mLinking archive library $@\ - @mkdir -p $(@D) - $(AR) rcs $@ $(OBJECTS) - -$(LIBRARYDIRECTORY)/Release/libViconDataStreamSDKCore.a: makefile $(OBJECTS) - @echo \[1\;35mLinking archive library $@\ - @mkdir -p $(@D) - $(AR) rcs $@ $(OBJECTS) - -# Source Files -$(CONFIG)/ClientUtils.o: makefile $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/ClientUtils.cpp - @echo \[1\;34mCompiling ClientUtils.cpp\ - @mkdir -p $(@D) - find $(CONFIG) -name *.gch -exec cp '{}' . \; - $(CXX) -fPIC -MMD -MP -I$(CONFIG)/ $(CXXFLAGS) -o $@ -c $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/ClientUtils.cpp - --include $(CONFIG)/ClientUtils.d - -$(CONFIG)/CoreClient.o: makefile $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/CoreClient.cpp - @echo \[1\;34mCompiling CoreClient.cpp\ - @mkdir -p $(@D) - find $(CONFIG) -name *.gch -exec cp '{}' . \; - $(CXX) -fPIC -MMD -MP -I$(CONFIG)/ $(CXXFLAGS) -o $@ -c $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/CoreClient.cpp - --include $(CONFIG)/CoreClient.d - -$(CONFIG)/AxisMapping.o: makefile $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/AxisMapping.cpp - @echo \[1\;34mCompiling AxisMapping.cpp\ - @mkdir -p $(@D) - find $(CONFIG) -name *.gch -exec cp '{}' . \; - $(CXX) -fPIC -MMD -MP -I$(CONFIG)/ $(CXXFLAGS) -o $@ -c $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/AxisMapping.cpp - --include $(CONFIG)/AxisMapping.d - -$(CONFIG)/RetimingClient.o: makefile $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/RetimingClient.cpp - @echo \[1\;34mCompiling RetimingClient.cpp\ - @mkdir -p $(@D) - find $(CONFIG) -name *.gch -exec cp '{}' . \; - $(CXX) -fPIC -MMD -MP -I$(CONFIG)/ $(CXXFLAGS) -o $@ -c $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/RetimingClient.cpp - --include $(CONFIG)/RetimingClient.d - -$(CONFIG)/RetimerUtils.o: makefile $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/RetimerUtils.cpp - @echo \[1\;34mCompiling RetimerUtils.cpp\ - @mkdir -p $(@D) - find $(CONFIG) -name *.gch -exec cp '{}' . \; - $(CXX) -fPIC -MMD -MP -I$(CONFIG)/ $(CXXFLAGS) -o $@ -c $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconDataStreamSDKCore/RetimerUtils.cpp - --include $(CONFIG)/RetimerUtils.d - -# Header Files -# Other Files - -clean: - @echo \[1\;31mCleaning $(CONFIG) build\ - find . -path '*/$(CONFIG)/*' \( -name '*.[od]' -o -name '*.gch' \) -exec rm -f {} ';' - rm -f moc_*.cxx - -$(HIDE_BOOST_SCRIPT): makefile - echo -n >$@ - echo "{" >>$@ - echo " local: *N5boost*; *NK5boost*;" >>$@ - echo "};" >>$@ diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/CClient.cpp b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/CClient.cpp deleted file mode 100644 index 74fe9deac..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/CClient.cpp +++ /dev/null @@ -1,1046 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#include "CClient.h" -#include -#include - -using namespace ViconDataStreamSDK::CPP; - - -#ifdef _WIN32 -#define snprintf(str,size,format,arg) _snprintf_s(str,size,_TRUNCATE,format,arg) -#endif - -CClient* Client_Create() -{ - Client* client = new Client(); - return client; -} - -void Client_Destroy(CClient* client) -{ - if(client) - { - delete ((Client*) client); - } -} - -void Client_GetVersion(CClient* client, COutput_GetVersion* outptr) -{ - const Output_GetVersion& outp = ((Client*) client)->GetVersion(); - /*COutput_GetVersion c_outp = {outp.Major,outp.Minor,outp.Point}; - return c_outp;*/ - outptr->Major=outp.Major; outptr->Minor=outp.Minor; outptr->Point=outp.Point; -} - -CEnum Client_Connect(CClient* client, CString HostName ) -{ - //COutput_Connect outp = {((Client*) client)->Connect(String(HostName)).Result}; - //return outp; - return ((Client*) client)->Connect(String(HostName)).Result; -} - -CEnum Client_ConnectToMulticast(CClient* client, CString LocalIP, CString MulticastIP ) -{ - return ((Client*) client)->ConnectToMulticast(String(LocalIP), String(MulticastIP)).Result; -} - -CEnum Client_Disconnect(CClient* client) -{ - return ((Client*) client)->Disconnect().Result; -} - -CBool Client_IsConnected(CClient* client) -{ - //COutput_IsConnected outp = { ((Client*) client)->IsConnected().Connected}; - //return outp; - //*((CBool*)outptr) = ((Client*) client)->IsConnected().Connected; - return ((Client*) client)->IsConnected().Connected; -} - -CEnum Client_StartTransmittingMulticast(CClient* client, CString ServerIP, - CString MulticastIP ) -{ - return ((Client*) client)->StartTransmittingMulticast(String(ServerIP), String(MulticastIP)).Result; -} - -CEnum Client_StopTransmittingMulticast(CClient* client) -{ - return ((Client*) client)->StopTransmittingMulticast().Result; -} - -CEnum Client_EnableSegmentData(CClient* client) -{ - return ((Client*) client)->EnableSegmentData().Result; -} - -CEnum Client_EnableMarkerData(CClient* client) -{ - return ((Client*) client)->EnableMarkerData().Result; -} - -CEnum Client_EnableUnlabeledMarkerData(CClient* client) -{ - return ((Client*) client)->EnableUnlabeledMarkerData().Result; -} -CEnum Client_EnableDeviceData(CClient* client) -{ - return ((Client*) client)->EnableDeviceData().Result; -} - -CEnum Client_DisableSegmentData(CClient* client) -{ - return ((Client*) client)->DisableSegmentData().Result; -} - -CEnum Client_DisableMarkerData(CClient* client) -{ - return ((Client*) client)->DisableMarkerData().Result; -} - -CEnum Client_DisableUnlabeledMarkerData(CClient* client) -{ - return ((Client*) client)->DisableUnlabeledMarkerData().Result; -} - -CEnum Client_DisableDeviceData(CClient* client) -{ - return ((Client*) client)->DisableDeviceData().Result; -} - -CBool Client_IsSegmentDataEnabled(CClient* client) -{ - return ((Client*) client)->IsSegmentDataEnabled().Enabled; -} - -CBool Client_IsMarkerDataEnabled(CClient* client) -{ - return ((Client*) client)->IsMarkerDataEnabled().Enabled; -} - -CBool Client_IsUnlabeledMarkerDataEnabled(CClient* client) -{ - return ((Client*) client)->IsUnlabeledMarkerDataEnabled().Enabled; -} - -CBool Client_IsDeviceDataEnabled(CClient* client) -{ - return ((Client*) client)->IsDeviceDataEnabled().Enabled; -} - -CEnum Client_SetStreamMode(CClient* client, CEnum Mode ) -{ - return ((Client*) client)->SetStreamMode((StreamMode::Enum) Mode).Result; -} - -CEnum Client_SetApexDeviceFeedback(CClient* client, CString i_rDeviceName, CBool i_bOn ) -{ - return ((Client*) client)->SetApexDeviceFeedback(String(i_rDeviceName), i_bOn!=0).Result; -} - -CEnum Client_SetAxisMapping(CClient* client, CEnum XAxis, CEnum YAxis, CEnum ZAxis ) -{ - return ((Client*) client)->SetAxisMapping((Direction::Enum) XAxis, (Direction::Enum) YAxis, - (Direction::Enum) ZAxis).Result; -} -void Client_GetAxisMapping(CClient* client, COutput_GetAxisMapping* outptr) -{ - const Output_GetAxisMapping& outp = ((Client*) client)->GetAxisMapping(); - /*COutput_GetAxisMapping c_outp = {outp.XAxis,outp.YAxis,outp.ZAxis}; - return c_outp;*/ - outptr->XAxis = outp.XAxis; - outptr->YAxis = outp.YAxis; - outptr->ZAxis = outp.ZAxis; -} - -CEnum Client_GetFrame(CClient* client) -{ - return ((Client*) client)->GetFrame().Result; -} - -void Client_GetFrameNumber(CClient* client, COutput_GetFrameNumber* outptr) -{ - const Output_GetFrameNumber& outp = ((Client*) client)->GetFrameNumber(); - /*COutput_GetFrameNumber c_outp = {outp.Result, outp.FrameNumber}; - return c_outp;*/ - outptr->Result = outp.Result; - outptr->FrameNumber = outp.FrameNumber; -} - -void Client_GetTimecode(CClient* client, COutput_GetTimecode* outptr) -{ - const Output_GetTimecode& outp = ((Client*) client)->GetTimecode(); - /*COutput_GetTimecode c_outp = {outp.Result,outp.Hours,outp.Minutes,outp.Seconds, - outp.Frames, outp.SubFrame, outp.FieldFlag, outp.Standard, - outp.SubFramesPerFrame, outp.UserBits}; - return c_outp;*/ - outptr->Result=outp.Result; outptr->Hours=outp.Hours; - outptr->Minutes=outp.Minutes; outptr->Seconds=outp.Seconds; - outptr->Frames=outp.Frames; outptr->SubFrame=outp.SubFrame; - outptr->FieldFlag=outp.FieldFlag; outptr->Standard=outp.Standard; - outptr->SubFramesPerFrame=outp.SubFramesPerFrame; outptr->UserBits=outp.UserBits; -} - -void Client_GetFrameRate(CClient* client, COutput_GetFrameRate* outptr) -{ - const Output_GetFrameRate& outp = ((Client*) client)->GetFrameRate(); - /*COutput_GetFrameRate c_outp = {outp.Result,outp.FrameRateHz}; - return c_outp;*/ - outptr->Result = outp.Result; - outptr->FrameRateHz = outp.FrameRateHz; -} - -void Client_GetLatencySampleCount(CClient* client, COutput_GetLatencySampleCount* outptr) -{ - const Output_GetLatencySampleCount& outp = ((Client*) client)->GetLatencySampleCount(); - /*COutput_GetLatencySampleCount c_outp = {outp.Result,outp.Count}; - return c_outp;*/ - outptr->Result = outp.Result; - outptr->Count = outp.Count; -} -CEnum Client_GetLatencySampleName(CClient* client, unsigned int LatencySampleIndex, int sizeOfBuffer, char* outstr ) -{ - const Output_GetLatencySampleName& outp = ((Client*) client)->GetLatencySampleName(LatencySampleIndex); - /*COutput_GetLatencySampleName c_outp = {outp.Result,std::string(outp.Name).c_str()}; - return c_outp;*/ - /*outptr->Result = outp.Result; - outptr->Name = std::string(outp.Name).c_str();*/ - snprintf(outstr, sizeOfBuffer, "%s", std::string(outp.Name).c_str()); - return outp.Result; -} -void Client_GetLatencySampleValue(CClient* client, CString LatencySampleName, COutput_GetLatencySampleValue* outptr ) -{ - const Output_GetLatencySampleValue& outp = ((Client*) client)->GetLatencySampleValue(String(LatencySampleName)); - /*COutput_GetLatencySampleValue c_outp = {outp.Result,outp.Value}; - return c_outp;*/ - outptr->Result = outp.Result; - outptr->Value = outp.Value; -} -void Client_GetLatencyTotal(CClient* client, COutput_GetLatencyTotal* outptr) -{ - const Output_GetLatencyTotal& outp = ((Client*) client)->GetLatencyTotal(); - /*COutput_GetLatencyTotal c_outp = {outp.Result,outp.Total}; - return c_outp;*/ - outptr->Result = outp.Result; - outptr->Total = outp.Total; -} - -void Client_GetSubjectCount(CClient* client, COutput_GetSubjectCount* outptr) -{ - const Output_GetSubjectCount& outp = ((Client*) client)->GetSubjectCount(); - /*COutput_GetSubjectCount c_outp = {outp.Result,outp.SubjectCount}; - return c_outp;*/ - outptr->Result = outp.Result; - outptr->SubjectCount = outp.SubjectCount; -} - -CEnum Client_GetSubjectName(CClient* client, unsigned int SubjectIndex, int sizeOfBuffer, char* outstr ) -{ - const Output_GetSubjectName& outp = ((Client*) client)->GetSubjectName(SubjectIndex); - /*COutput_GetSubjectName c_outp = {outp.Result, std::string(outp.SubjectName).c_str()}; - return c_outp;*/ - /*outptr->Result = outp.Result; - outptr->SubjectName = std::string(outp.SubjectName).c_str();*/ - - /*static char dummy[] = "dummy"; - outptr->SubjectName = dummy;*/ - snprintf(outstr, sizeOfBuffer, "%s", std::string(outp.SubjectName).c_str()); - return outp.Result; -} - -CEnum Client_GetSubjectRootSegmentName(CClient* client, CString SubjectName, int sizeOfBuffer, char* outstr ) -{ - const Output_GetSubjectRootSegmentName& outp = ((Client*) client)->GetSubjectRootSegmentName(String(SubjectName)); - /*COutput_GetSubjectRootSegmentName c_outp = {outp.Result, std::string(outp.SegmentName).c_str()}; - return c_outp;*/ - /*outptr->Result = outp.Result; - outptr->SegmentName = std::string(outp.SegmentName).c_str();*/ - snprintf(outstr, sizeOfBuffer, "%s", std::string(outp.SegmentName).c_str()); - return outp.Result; -} - -void Client_GetSegmentCount( CClient* client, CString SubjectName, COutput_GetSegmentCount* outptr ) -{ - const Output_GetSegmentCount& outp = ((Client*) client)->GetSegmentCount(String(SubjectName)); - /*COutput_GetSegmentCount c_outp = {outp.Result,outp.SegmentCount}; - return c_outp;*/ - outptr->Result = outp.Result; - outptr->SegmentCount = outp.SegmentCount; -} - -CEnum Client_GetSegmentName(CClient* client, CString SubjectName, - unsigned int SegmentIndex, int sizeOfBuffer, char* outstr ) -{ - const Output_GetSegmentName& outp = ((Client*) client)->GetSegmentName(String(SubjectName),SegmentIndex); - /*COutput_GetSegmentName c_outp = {outp.Result, std::string(outp.SegmentName).c_str()}; - return c_outp; */ - /*outptr->Result = outp.Result; - outptr->SegmentName = std::string(outp.SegmentName).c_str();*/ - snprintf(outstr, sizeOfBuffer, "%s", std::string(outp.SegmentName).c_str()); - return outp.Result; -} - -void Client_GetSegmentChildCount(CClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentChildCount* outptr ) -{ - const Output_GetSegmentChildCount& outp = ((Client*) client)->GetSegmentChildCount(String(SubjectName), - String(SegmentName)); - /*COutput_GetSegmentChildCount c_outp = {outp.Result,outp.SegmentCount}; - return c_outp;*/ - outptr->Result = outp.Result; - outptr->SegmentCount = outp.SegmentCount; -} - -CEnum Client_GetSegmentChildName(CClient* client, CString SubjectName, - CString SegmentName, - unsigned int SegmentIndex, - int sizeOfBuffer, char* outstr) -{ - const Output_GetSegmentChildName& outp = ((Client*) client)->GetSegmentChildName(String(SubjectName), - String(SegmentName), SegmentIndex); - /*COutput_GetSegmentChildName c_outp = {outp.Result, std::string(outp.SegmentName).c_str()}; - return c_outp;*/ - /*outptr->Result = outp.Result; - outptr->SegmentName = std::string(outp.SegmentName).c_str();*/ - snprintf(outstr, sizeOfBuffer, "%s", std::string(outp.SegmentName).c_str()); - return outp.Result; -} - -CEnum Client_GetSegmentParentName(CClient* client, CString SubjectName, - CString SegmentName, int sizeOfBuffer, char* outstr ) -{ - const Output_GetSegmentParentName& outp = ((Client*) client)->GetSegmentParentName(String(SubjectName), - String(SegmentName)); - /*COutput_GetSegmentParentName c_outp = {outp.Result,std::string(outp.SegmentName).c_str()}; - return c_outp;*/ - /*outptr->Result = outp.Result; - outptr->SegmentName = std::string(outp.SegmentName).c_str();*/ - snprintf(outstr, sizeOfBuffer, "%s", std::string(outp.SegmentName).c_str()); - return outp.Result; -} - -void Client_GetSegmentStaticTranslation(CClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentStaticTranslation* outptr ) -{ - const Output_GetSegmentStaticTranslation& outp = ((Client*) client)->GetSegmentStaticTranslation(String(SubjectName), - String(SegmentName)); - /*double* ptr = outp.Translation; - COutput_GetSegmentStaticTranslation c_outp = {outp.Result, *ptr++,*ptr++,*ptr}; - return c_outp;*/ - outptr->Result = outp.Result; - std::memcpy(outptr->Translation,outp.Translation,sizeof(outptr->Translation)); -} - -void Client_GetSegmentStaticRotationHelical(CClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentStaticRotationHelical* outptr ) -{ - const Output_GetSegmentStaticRotationHelical& outp = ((Client*) client)->GetSegmentStaticRotationHelical( - String(SubjectName), String(SegmentName)); - /*double* ptr = outp.Rotation; - COutput_GetSegmentStaticRotationHelical c_outp = {outp.Result, *ptr++,*ptr++,*ptr}; - return c_outp;*/ - outptr->Result = outp.Result; - std::memcpy(outptr->Rotation,outp.Rotation,sizeof(outptr->Rotation)); -} - -void Client_GetSegmentStaticRotationMatrix(CClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentStaticRotationMatrix* outptr ) -{ - const Output_GetSegmentStaticRotationMatrix& outp = ((Client*) client)->GetSegmentStaticRotationMatrix( - String(SubjectName), String(SegmentName)); - /*double* ptr = outp.Rotation; - COutput_GetSegmentStaticRotationMatrix c_outp = {outp.Result,*ptr++,*ptr++,*ptr++, - *ptr++,*ptr++,*ptr++ - *ptr++,*ptr++,*ptr};*/ - /*COutput_GetSegmentStaticRotationMatrix c_outp; - c_outp.Result = outp.Result; - std::memcpy(c_outp.Rotation,outp.Rotation,sizeof(c_outp.Rotation)); - return c_outp;*/ - outptr->Result = outp.Result; - std::memcpy(outptr->Rotation,outp.Rotation,sizeof(outptr->Rotation)); -} - -void Client_GetSegmentStaticRotationQuaternion(CClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentStaticRotationQuaternion* outptr ) -{ - const Output_GetSegmentStaticRotationQuaternion& outp = ((Client*) client)->GetSegmentStaticRotationQuaternion( - String(SubjectName), String(SegmentName)); - /*double* ptr = outp.Rotation; - COutput_GetSegmentStaticRotationQuaternion c_outp = {outp.Result,*ptr++,*ptr++,*ptr++,*ptr}; - return c_outp;*/ - outptr->Result = outp.Result; - std::memcpy(outptr->Rotation,outp.Rotation,sizeof(outptr->Rotation)); -} - -void Client_GetSegmentStaticRotationEulerXYZ(CClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentStaticRotationEulerXYZ* outptr ) -{ - const Output_GetSegmentStaticRotationEulerXYZ& outp = ((Client*) client)->GetSegmentStaticRotationEulerXYZ( - String(SubjectName), String(SegmentName)); - /*double* ptr = outp.Rotation; - COutput_GetSegmentStaticRotationEulerXYZ c_outp = {outp.Result,*ptr++,*ptr++,*ptr}; - return c_outp;*/ - outptr->Result = outp.Result; - std::memcpy(outptr->Rotation,outp.Rotation,sizeof(outptr->Rotation)); -} - -void Client_GetSegmentGlobalTranslation(CClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentGlobalTranslation* outptr ) -{ - const Output_GetSegmentGlobalTranslation& outp = ((Client*) client)->GetSegmentGlobalTranslation( - String(SubjectName), String(SegmentName)); - /*double* ptr = outp.Translation; - COutput_GetSegmentGlobalTranslation c_outp = {outp.Result,*ptr++,*ptr++,*ptr,outp.Occluded}; - return c_outp;*/ - outptr->Result = outp.Result; - std::memcpy(outptr->Translation,outp.Translation,sizeof(outptr->Translation)); - outptr->Occluded = outp.Occluded; -} - -void Client_GetSegmentGlobalRotationHelical(CClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentGlobalRotationHelical* outptr ) -{ - const Output_GetSegmentGlobalRotationHelical& outp = ((Client*) client)->GetSegmentGlobalRotationHelical( - String(SubjectName), String(SegmentName)); - /*double* ptr = outp.Rotation; - COutput_GetSegmentGlobalRotationHelical c_outp = {outp.Result,*ptr++,*ptr++,*ptr,outp.Occluded}; - return c_outp;*/ - outptr->Result = outp.Result; - std::memcpy(outptr->Rotation,outp.Rotation,sizeof(outptr->Rotation)); - outptr->Occluded = outp.Occluded; -} - -void Client_GetSegmentGlobalRotationMatrix(CClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentGlobalRotationMatrix* outptr ) -{ - const Output_GetSegmentGlobalRotationMatrix& outp = ((Client*) client)->GetSegmentGlobalRotationMatrix( - String(SubjectName), String(SegmentName)); - /*COutput_GetSegmentGlobalRotationMatrix c_outp; - c_outp.Result = outp.Result; - std::memcpy(c_outp.Rotation, outp.Rotation, sizeof(c_outp.Rotation)); - c_outp.Occluded = outp.Occluded; - return c_outp;*/ - outptr->Result = outp.Result; - std::memcpy(outptr->Rotation,outp.Rotation,sizeof(outptr->Rotation)); - outptr->Occluded = outp.Occluded; -} - -void Client_GetSegmentGlobalRotationQuaternion(CClient* client, CString SubjectName, CString SegmentName, - COutput_GetSegmentGlobalRotationQuaternion* outptr) -{ - const Output_GetSegmentGlobalRotationQuaternion& outp = ((Client*) client)->GetSegmentGlobalRotationQuaternion( - String(SubjectName), String(SegmentName)); - /*double* ptr = outp.Rotation; - COutput_GetSegmentGlobalRotationQuaternion c_outp = {outp.Result,*ptr++,*ptr++,*ptr++,*ptr,outp.Occluded}; - return c_outp;*/ - outptr->Result = outp.Result; - //outp.Rotation[0]=1.0; outp.Rotation[1]=2.0;outp.Rotation[2]=3.0;outp.Rotation[3]=4.0; - std::memcpy(outptr->Rotation,outp.Rotation,sizeof(outptr->Rotation)); - //double* from = outp.Rotation, *to = outptr->Rotation; - //*to++=*from++;*to++=*from++;*to++=*from++;*to=*from; - outptr->Occluded = outp.Occluded; -} - - -void Client_GetSegmentGlobalRotationEulerXYZ(CClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentGlobalRotationEulerXYZ* outptr ) -{ - const Output_GetSegmentGlobalRotationEulerXYZ& outp = ((Client*) client)->GetSegmentGlobalRotationEulerXYZ( - String(SubjectName), String(SegmentName)); - /*double* ptr = outp.Rotation; - COutput_GetSegmentGlobalRotationEulerXYZ c_outp = {outp.Result,*ptr++,*ptr++,*ptr,outp.Occluded}; - return c_outp;*/ - outptr->Result = outp.Result; - std::memcpy(outptr->Rotation,outp.Rotation,sizeof(outptr->Rotation)); - outptr->Occluded = outp.Occluded; -} - -void Client_GetSegmentLocalTranslation(CClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentLocalTranslation* outptr ) -{ - const Output_GetSegmentLocalTranslation& outp = ((Client*) client)->GetSegmentLocalTranslation( - String(SubjectName), String(SegmentName)); - /*double* ptr = outp.Translation; - COutput_GetSegmentLocalTranslation c_outp = {outp.Result,*ptr++,*ptr++,*ptr,outp.Occluded}; - return c_outp;*/ - outptr->Result = outp.Result; - std::memcpy(outptr->Translation,outp.Translation,sizeof(outptr->Translation)); - outptr->Occluded = outp.Occluded; -} - -void Client_GetSegmentLocalRotationHelical(CClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentLocalRotationHelical* outptr ) -{ - const Output_GetSegmentLocalRotationHelical& outp = ((Client*) client)->GetSegmentLocalRotationHelical( - String(SubjectName), String(SegmentName)); - /*double* ptr = outp.Rotation; - COutput_GetSegmentLocalRotationHelical c_outp = {outp.Result,*ptr++,*ptr++,*ptr,outp.Occluded}; - return c_outp;*/ - outptr->Result = outp.Result; - std::memcpy(outptr->Rotation,outp.Rotation,sizeof(outptr->Rotation)); - outptr->Occluded = outp.Occluded; -} - -void Client_GetSegmentLocalRotationMatrix(CClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentLocalRotationMatrix* outptr ) -{ - const Output_GetSegmentLocalRotationMatrix& outp = ((Client*) client)->GetSegmentLocalRotationMatrix( - String(SubjectName), String(SegmentName)); - /*COutput_GetSegmentLocalRotationMatrix c_outp; - c_outp.Result = outp.Result; - std::memcpy(c_outp.Rotation,outp.Rotation,sizeof(c_outp.Rotation)); - c_outp.Occluded = outp.Occluded; - return c_outp;*/ - outptr->Result = outp.Result; - std::memcpy(outptr->Rotation,outp.Rotation,sizeof(outptr->Rotation)); - outptr->Occluded = outp.Occluded; -} - -void Client_GetSegmentLocalRotationQuaternion(CClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentLocalRotationQuaternion* outptr ) -{ - const Output_GetSegmentLocalRotationQuaternion& outp = ((Client*) client)->GetSegmentLocalRotationQuaternion( - String(SubjectName), String(SegmentName)); - /*double* ptr = outp.Rotation; - COutput_GetSegmentLocalRotationQuaternion c_outp = {outp.Result,*ptr++,*ptr++,*ptr++,*ptr,outp.Occluded}; - return c_outp;*/ - outptr->Result = outp.Result; - std::memcpy(outptr->Rotation,outp.Rotation,sizeof(outptr->Rotation)); - outptr->Occluded = outp.Occluded; -} - -void Client_GetSegmentLocalRotationEulerXYZ(CClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentLocalRotationEulerXYZ* outptr ) -{ - const Output_GetSegmentLocalRotationEulerXYZ& outp = ((Client*) client)->GetSegmentLocalRotationEulerXYZ( - String(SubjectName), String(SegmentName)); - /*double* ptr = outp.Rotation; - COutput_GetSegmentLocalRotationEulerXYZ c_outp = {outp.Result,*ptr++,*ptr++,*ptr,outp.Occluded}; - return c_outp;*/ - outptr->Result = outp.Result; - std::memcpy(outptr->Rotation,outp.Rotation,sizeof(outptr->Rotation)); - outptr->Occluded = outp.Occluded; -} - -void Client_GetMarkerCount(CClient* client, CString SubjectName, COutput_GetMarkerCount* outptr ) -{ - const Output_GetMarkerCount& outp = ((Client*) client)->GetMarkerCount(String(SubjectName)); - /*COutput_GetMarkerCount c_outp = {outp.Result,outp.MarkerCount}; - return c_outp;*/ - outptr->Result = outp.Result; - outptr->MarkerCount = outp.MarkerCount; -} - -CEnum Client_GetMarkerName(CClient* client, CString SubjectName, - unsigned int MarkerIndex, int sizeOfBuffer, char* outstr ) -{ - const Output_GetMarkerName& outp = ((Client*) client)->GetMarkerName(String(SubjectName),MarkerIndex); - /*COutput_GetMarkerName c_outp = {outp.Result, std::string(outp.MarkerName).c_str()}; - return c_outp;*/ - /*outptr->Result = outp.Result; - outptr->MarkerName = std::string(outp.MarkerName).c_str();*/ - snprintf(outstr, sizeOfBuffer, "%s", std::string(outp.MarkerName).c_str()); - return outp.Result; -} - -CEnum Client_GetMarkerParentName(CClient* client, CString SubjectName, - CString MarkerName, int sizeOfBuffer, char* outstr ) -{ - const Output_GetMarkerParentName& outp = ((Client*) client)->GetMarkerParentName( - String(SubjectName), String(MarkerName)); - /*COutput_GetMarkerParentName c_outp = {outp.Result,std::string(outp.SegmentName).c_str()}; - return c_outp;*/ - /*outptr->Result = outp.Result; - outptr->SegmentName = std::string(outp.SegmentName).c_str();*/ - snprintf(outstr, sizeOfBuffer, "%s", std::string(outp.SegmentName).c_str()); - return outp.Result; -} - -void Client_GetMarkerGlobalTranslation(CClient* client, CString SubjectName, - CString MarkerName, COutput_GetMarkerGlobalTranslation* outptr ) -{ - const Output_GetMarkerGlobalTranslation& outp = ((Client*) client)->GetMarkerGlobalTranslation( - String(SubjectName), String(MarkerName)); - /*double* ptr = outp.Translation; - COutput_GetMarkerGlobalTranslation c_outp = {outp.Result,*ptr++,*ptr++,*ptr,outp.Occluded}; - return c_outp;*/ - outptr->Result = outp.Result; - std::memcpy(outptr->Translation,outp.Translation,sizeof(outptr->Translation)); - outptr->Occluded = outp.Occluded; -} - -void Client_GetUnlabeledMarkerCount(CClient* client, COutput_GetUnlabeledMarkerCount* outptr) -{ - const Output_GetUnlabeledMarkerCount& outp = ((Client*) client)->GetUnlabeledMarkerCount(); - /*COutput_GetUnlabeledMarkerCount c_outp = {outp.Result, outp.MarkerCount}; - return c_outp;*/ - outptr->Result = outp.Result; - outptr->MarkerCount = outp.MarkerCount; -} - -void Client_GetUnlabeledMarkerGlobalTranslation(CClient* client, unsigned int MarkerIndex, - COutput_GetUnlabeledMarkerGlobalTranslation* outptr) -{ - const Output_GetUnlabeledMarkerGlobalTranslation& outp = ((Client*) client)->GetUnlabeledMarkerGlobalTranslation(MarkerIndex); - /*double* ptr = outp.Translation; - COutput_GetUnlabeledMarkerGlobalTranslation c_outp = {outp.Result,*ptr++,*ptr++,*ptr}; - return c_outp;*/ - outptr->Result = outp.Result; - std::memcpy(outptr->Translation,outp.Translation,sizeof(outptr->Translation)); -} - -void Client_GetDeviceCount(CClient* client, COutput_GetDeviceCount* outptr) -{ - const Output_GetDeviceCount& outp = ((Client*) client)->GetDeviceCount(); - /*COutput_GetDeviceCount c_outp = {outp.Result, outp.DeviceCount}; - return c_outp;*/ - outptr->Result = outp.Result; - outptr->DeviceCount = outp.DeviceCount; -} - -CEnum Client_GetDeviceName(CClient* client, unsigned int DeviceIndex, - int sizeOfBuffer, char* outstr, CEnum* DeviceType ) -{ - const Output_GetDeviceName& outp = ((Client*) client)->GetDeviceName(DeviceIndex); - /*COutput_GetDeviceName c_outp = {outp.Result,std::string(outp.DeviceName).c_str(),outp.DeviceType}; - return c_outp;*/ - /*outptr->Result = outp.Result; - outptr->DeviceName = std::string(outp.DeviceName).c_str(); - outptr->DeviceType = outp.DeviceType;*/ - snprintf(outstr, sizeOfBuffer, "%s", std::string(outp.DeviceName).c_str()); - *DeviceType = outp.DeviceType; - return outp.Result; -} - -void Client_GetDeviceOutputCount(CClient* client, CString DeviceName, COutput_GetDeviceOutputCount* outptr ) -{ - const Output_GetDeviceOutputCount& outp = ((Client*) client)->GetDeviceOutputCount(String(DeviceName)); - /*COutput_GetDeviceOutputCount c_outp = {outp.Result,outp.DeviceOutputCount}; - return c_outp;*/ - outptr->Result = outp.Result; - outptr->DeviceOutputCount = outp.DeviceOutputCount; -} - -CEnum Client_GetDeviceOutputName(CClient* client, CString DeviceName, - unsigned int DeviceOutputIndex, int sizeOfBuffer, char* outstr, CEnum* DeviceOutputUnit ) -{ - const Output_GetDeviceOutputName& outp = ((Client*) client)->GetDeviceOutputName(String(DeviceName), - DeviceOutputIndex); - /*COutput_GetDeviceOutputName c_outp = {outp.Result,std::string(outp.DeviceOutputName).c_str(),outp.DeviceOutputUnit}; - return c_outp;*/ - /*outptr->Result = outp.Result; - outptr->DeviceOutputName = std::string(outp.DeviceOutputName).c_str(); - outptr->DeviceOutputUnit = outp.DeviceOutputUnit;*/ - snprintf(outstr, sizeOfBuffer, "%s", std::string(outp.DeviceOutputName).c_str()); - *DeviceOutputUnit = outp.DeviceOutputUnit; - return outp.Result; -} - -void Client_GetDeviceOutputValue(CClient* client, CString DeviceName, - CString DeviceOutputName, COutput_GetDeviceOutputValue* outptr ) -{ - const Output_GetDeviceOutputValue& outp = ((Client*) client)->GetDeviceOutputValue(String(DeviceName), - String(DeviceOutputName)); - /*COutput_GetDeviceOutputValue c_outp = {outp.Result,outp.Value,outp.Occluded}; - return c_outp;*/ - outptr->Result = outp.Result; - outptr->Value = outp.Value; - outptr->Occluded = outp.Occluded; -} - -void Client_GetDeviceOutputSubsamples(CClient* client, CString DeviceName, - CString DeviceOutputName, COutput_GetDeviceOutputSubsamples* outptr ) -{ - const Output_GetDeviceOutputSubsamples& outp = ((Client*) client)->GetDeviceOutputSubsamples( - String(DeviceName), String(DeviceOutputName)); - /*COutput_GetDeviceOutputSubsamples c_outp = {outp.Result,outp.DeviceOutputSubsamples,outp.Occluded}; - return c_outp;*/ - outptr->Result = outp.Result; - outptr->DeviceOutputSubsamples = outp.DeviceOutputSubsamples; - outptr->Occluded = outp.Occluded; -} - -void Client_GetDeviceOutputValueForSubsample(CClient* client, CString DeviceName, - CString DeviceOutputName, - unsigned int Subsample, - COutput_GetDeviceOutputValue* outptr) -{ - const Output_GetDeviceOutputValue& outp = ((Client*) client)->GetDeviceOutputValue( - String(DeviceName), String(DeviceOutputName),Subsample); - /*COutput_GetDeviceOutputValue c_outp = {outp.Result,outp.Value,outp.Occluded}; - return c_outp;*/ - outptr->Result = outp.Result; - outptr->Value = outp.Value; - outptr->Occluded = outp.Occluded; - -} - -void Client_GetForcePlateCount(CClient* client, COutput_GetForcePlateCount* outptr) -{ - const Output_GetForcePlateCount& outp = ((Client*) client)->GetForcePlateCount(); - /*COutput_GetForcePlateCount c_outp = {outp.Result,outp.ForcePlateCount}; - return c_outp;*/ - outptr->Result = outp.Result; - outptr->ForcePlateCount = outp.ForcePlateCount; -} - -void Client_GetGlobalForceVector(CClient* client, unsigned int ForcePlateIndex, COutput_GetGlobalForceVector* outptr ) -{ - const Output_GetGlobalForceVector& outp = ((Client*) client)->GetGlobalForceVector(ForcePlateIndex); - /*double* ptr = outp.ForceVector; - COutput_GetGlobalForceVector c_outp = {outp.Result,*ptr++,*ptr++,*ptr}; - return c_outp;*/ - outptr->Result = outp.Result; - std::memcpy(outptr->ForceVector,outp.ForceVector,sizeof(outptr->ForceVector)); -} - -void Client_GetGlobalMomentVector(CClient* client, unsigned int ForcePlateIndex, COutput_GetGlobalMomentVector* outptr ) -{ - const Output_GetGlobalMomentVector& outp = ((Client*) client)->GetGlobalMomentVector(ForcePlateIndex); - /*double* ptr = outp.MomentVector; - COutput_GetGlobalMomentVector c_outp = {outp.Result,*ptr++,*ptr++,*ptr}; - return c_outp;*/ - outptr->Result = outp.Result; - std::memcpy(outptr->MomentVector,outp.MomentVector,sizeof(outptr->MomentVector)); -} - -void Client_GetGlobalCentreOfPressure(CClient* client, unsigned int ForcePlateIndex, COutput_GetGlobalCentreOfPressure* outptr ) -{ - const Output_GetGlobalCentreOfPressure& outp = ((Client*) client)->GetGlobalCentreOfPressure(ForcePlateIndex); - /*double* ptr = outp.CentreOfPressure; - COutput_GetGlobalCentreOfPressure c_outp = {outp.Result,*ptr++,*ptr++,*ptr}; - return c_outp;*/ - outptr->Result = outp.Result; - std::memcpy(outptr->CentreOfPressure,outp.CentreOfPressure,sizeof(outptr->CentreOfPressure)); -} - -void Client_GetForcePlateSubsamples(CClient* client, unsigned int ForcePlateIndex, COutput_GetForcePlateSubsamples* outptr ) -{ - const Output_GetForcePlateSubsamples& outp = ((Client*) client)->GetForcePlateSubsamples(ForcePlateIndex); - /*COutput_GetForcePlateSubsamples c_outp = {outp.Result, outp.ForcePlateSubsamples}; - return c_outp;*/ - outptr->Result = outp.Result; - outptr->ForcePlateSubsamples = outp.ForcePlateSubsamples; -} - -void Client_GetGlobalForceVectorForSubsample(CClient* client, unsigned int ForcePlateIndex, unsigned int Subsample, - COutput_GetGlobalForceVector* outptr) -{ - const Output_GetGlobalForceVector& outp = ((Client*) client)->GetGlobalForceVector(ForcePlateIndex,Subsample); - /*double* ptr = outp.ForceVector; - COutput_GetGlobalForceVector c_outp = {outp.Result,*ptr++,*ptr++,*ptr}; - return c_outp;*/ - outptr->Result = outp.Result; - std::memcpy(outptr->ForceVector,outp.ForceVector,sizeof(outptr->ForceVector)); -} - -void Client_GetGlobalMomentVectorForSubsample(CClient* client, unsigned int ForcePlateIndex, unsigned int Subsample, - COutput_GetGlobalMomentVector* outptr) -{ - const Output_GetGlobalMomentVector& outp = ((Client*) client)->GetGlobalMomentVector(ForcePlateIndex,Subsample); - /*double* ptr = outp.MomentVector; - COutput_GetGlobalMomentVector c_outp = {outp.Result,*ptr++,*ptr++,*ptr}; - return c_outp;*/ - outptr->Result = outp.Result; - std::memcpy(outptr->MomentVector,outp.MomentVector,sizeof(outptr->MomentVector)); -} - -void Client_GetGlobalCentreOfPressureForSubsample(CClient* client, unsigned int ForcePlateIndex, unsigned int Subsample, - COutput_GetGlobalCentreOfPressure* outptr) -{ - const Output_GetGlobalCentreOfPressure& outp = ((Client*) client)->GetGlobalCentreOfPressure(ForcePlateIndex,Subsample); - /*double* ptr = outp.CentreOfPressure; - COutput_GetGlobalCentreOfPressure c_outp = {outp.Result,*ptr++,*ptr++,*ptr}; - return c_outp;*/ - outptr->Result = outp.Result; - std::memcpy(outptr->CentreOfPressure,outp.CentreOfPressure,sizeof(outptr->CentreOfPressure)); -} - -void Client_GetEyeTrackerCount(CClient* client, COutput_GetEyeTrackerCount* outptr) -{ - const Output_GetEyeTrackerCount& outp = ((Client*) client)->GetEyeTrackerCount(); - /*COutput_GetEyeTrackerCount c_outp = {outp.Result,outp.EyeTrackerCount}; - return c_outp;*/ - outptr->Result = outp.Result; - outptr->EyeTrackerCount = outp.EyeTrackerCount; -} - -void Client_GetEyeTrackerGlobalPosition(CClient* client, unsigned int EyeTrackerIndex, - COutput_GetEyeTrackerGlobalPosition* outptr) -{ - const Output_GetEyeTrackerGlobalPosition& outp = ((Client*) client)->GetEyeTrackerGlobalPosition(EyeTrackerIndex); - /*double* ptr = outp.Position; - COutput_GetEyeTrackerGlobalPosition c_outp = {outp.Result,*ptr++,*ptr++,*ptr,outp.Occluded}; - return c_outp;*/ - outptr->Result = outp.Result; - std::memcpy(outptr->Position,outp.Position,sizeof(outptr->Position)); - outptr->Occluded = outp.Occluded; -} - -void Client_GetEyeTrackerGlobalGazeVector(CClient* client, unsigned int EyeTrackerIndex, - COutput_GetEyeTrackerGlobalGazeVector* outptr) -{ - const Output_GetEyeTrackerGlobalGazeVector& outp = ((Client*) client)->GetEyeTrackerGlobalGazeVector(EyeTrackerIndex); - /*double* ptr = outp.GazeVector; - COutput_GetEyeTrackerGlobalGazeVector c_outp = {outp.Result,*ptr++,*ptr++,*ptr,outp.Occluded}; - return c_outp;*/ - outptr->Result = outp.Result; - std::memcpy(outptr->GazeVector,outp.GazeVector,sizeof(outptr->GazeVector)); - outptr->Occluded = outp.Occluded; -} - -CEnum Client_EnableMarkerRayData( CClient* client ) -{ - return ((Client*) client)->EnableMarkerRayData().Result; -} - -CEnum Client_EnableCentroidData( CClient* client ) -{ - return ( (Client*)client )->EnableCentroidData().Result; -} - -CEnum Client_EnableGreyscaleData( CClient* client ) -{ - return NotImplemented; -} - -CEnum Client_EnableDebugData( CClient* client ) -{ - return ( (Client*)client )->EnableDebugData().Result; -} - -CEnum Client_DisableMarkerRayData( CClient* client ) -{ - return ( (Client*)client )->DisableMarkerRayData().Result; -} - -CEnum Client_DisableCentroidData( CClient* client ) -{ - return ( (Client*)client )->DisableCentroidData().Result; -} - -CEnum Client_DisableGreyscaleData( CClient* client ) -{ - return NotImplemented; -} - -CEnum Client_DisableDebugData( CClient* client ) -{ - return ( (Client*)client )->DisableDebugData().Result; -} - -CBool Client_IsMarkerRayDataEnabled( CClient* client ) -{ - return ((Client*) client)->IsMarkerRayDataEnabled().Enabled; -} - -CBool Client_IsCentroidDataEnabled( CClient* client ) -{ - return ( (Client*)client )->IsCentroidDataEnabled().Enabled; -} - -CBool Client_IsGreyscaleDataEnabled( CClient* client ) -{ - return false; -} - -CBool Client_IsDebugDataEnabled( CClient* client ) -{ - return ( (Client*)client )->IsDebugDataEnabled().Enabled; -} - -void Client_GetServerOrientation( CClient* client, COutput_GetServerOrientation* outptr ) -{ - Output_GetServerOrientation outpt = ((Client*) client)->GetServerOrientation(); - outptr->Orientation = outpt.Orientation; - outptr->Result = outpt.Result; -} - -void Client_GetHardwareFrameNumber( CClient* client, COutput_GetHardwareFrameNumber* outptr ) -{ - Output_GetHardwareFrameNumber outpt = ((Client*) client)->GetHardwareFrameNumber(); - outptr->HardwareFrameNumber = outpt.HardwareFrameNumber; - outptr->Result = outpt.Result; -} - -void Client_GetFrameRateCount( CClient* client, COutput_GetFrameRateCount* outptr ) -{ - Output_GetFrameRateCount outpt = ((Client*) client)->GetFrameRateCount(); - outptr->Count = outpt.Count; - outptr->Result = outpt.Result; -} - -CEnum Client_GetFrameRateName( CClient* client, unsigned int FrameRateIndex, int sizeOfBuffer, char* outstr ) -{ - Output_GetFrameRateName outpt = ((Client*) client)->GetFrameRateName( FrameRateIndex ); - snprintf( outstr, sizeOfBuffer, "%s", std::string( outpt.Name ).c_str() ); - return outpt.Result; -} - -void Client_GetFrameRateValue( CClient* client, CString FrameRateName, COutput_GetFrameRateValue* outptr ) -{ - Output_GetFrameRateValue outpt = ( (Client*)client )->GetFrameRateValue( (String)FrameRateName ); - outptr->Result = outpt.Result; - outptr->Value = outpt.Value; -} - -void Client_GetObjectQuality( CClient* client, CString ObjectName, COutput_GetObjectQuality* outptr ) -{ - Output_GetObjectQuality outpt = ((Client*) client)->GetObjectQuality( (String)ObjectName) ; - outptr->Result = outpt.Result; - outptr->Quality = outpt.Quality; -} - -void Client_GetMarkerRayContributionCount( CClient* client, CString SubjectName, CString MarkerName, COutput_GetMarkerRayContributionCount* outptr ) -{ - Output_GetMarkerRayContributionCount outpt = ( (Client*)client )->GetMarkerRayContributionCount( (String)SubjectName, (String)MarkerName ); - outptr->Result = outpt.Result; - outptr->RayContributionsCount = outpt.RayContributionsCount; -} - -void Client_GetMarkerRayContribution( CClient* client, CString SubjectName, CString MarkerName, unsigned int MarkerRayContributionIndex, COutput_GetMarkerRayContribution* outptr ) -{ - Output_GetMarkerRayContribution outpt = ( (Client*)client )->GetMarkerRayContribution( (String)SubjectName, (String)MarkerName, MarkerRayContributionIndex ); - outptr->Result = outpt.Result; - outptr->CentroidIndex = outpt.CentroidIndex; - outptr->CameraID = outpt.CameraID; -} - -void Client_GetLabeledMarkerCount( CClient* client, COutput_GetLabeledMarkerCount* outptr ) -{ - Output_GetLabeledMarkerCount outpt = ((Client*) client)->GetLabeledMarkerCount(); - outptr->Result = outpt.Result; - outptr->MarkerCount = outpt.MarkerCount; -} - -void Client_GetLabeledMarkerGlobalTranslation( CClient* client, unsigned int MarkerIndex, COutput_GetLabeledMarkerGlobalTranslation* outptr ) -{ - Output_GetLabeledMarkerGlobalTranslation outpt = ((Client*) client)->GetLabeledMarkerGlobalTranslation( MarkerIndex ); - outptr->Result = outpt.Result; - std::memcpy( outptr->Translation, outpt.Translation, sizeof( outptr->Translation ) ); -} - -void Client_GetCameraCount( CClient* client, COutput_GetCameraCount* outptr ) -{ - Output_GetCameraCount outpt = ((Client*) client)->GetCameraCount(); - outptr->Result = outpt.Result; - outptr->CameraCount = outpt.CameraCount; -} - -CEnum Client_GetCameraName( CClient* client, unsigned int i_CameraIndex, int sizeOfBuffer, char* outstr ) -{ - Output_GetCameraName outpt = ((Client*) client)->GetCameraName( i_CameraIndex ); - snprintf( outstr, sizeOfBuffer, "%s", std::string( outpt.CameraName ).c_str() ); - return outpt.Result; -} - -void Client_GetCameraId( CClient* client, CString i_rCameraName, COutput_GetCameraId* outptr ) -{ - Output_GetCameraId outpt = ((Client*) client)->GetCameraId( i_rCameraName ); - outptr->Result = outpt.Result; - outptr->CameraId = outpt.CameraId; -} - -void Client_GetCameraUserId( CClient* client, CString i_rCameraName, COutput_GetCameraUserId* outptr ) -{ - Output_GetCameraUserId outpt = ((Client*) client)->GetCameraUserId( (String)i_rCameraName ); - outptr->Result = outpt.Result; - outptr->CameraUserId = outpt.CameraUserId; -} - -CEnum Client_GetCameraType( CClient* client, CString i_rCameraName, int sizeOfBuffer, char* outstr ) -{ - Output_GetCameraType outpt = ((Client*) client)->GetCameraType( (String)i_rCameraName ); - snprintf( outstr, sizeOfBuffer, "%s", std::string( outpt.CameraType ).c_str() ); - return outpt.Result; -} - -CEnum Client_GetCameraDisplayName( CClient* client, CString i_rCameraName, int sizeOfBuffer, char* outstr ) -{ - Output_GetCameraDisplayName outpt = ((Client*) client)->GetCameraDisplayName( i_rCameraName ); - snprintf( outstr, sizeOfBuffer, "%s", std::string( outpt.CameraDisplayName ).c_str() ); - return outpt.Result; -} - -void Client_GetCameraResolution( CClient* client, CString i_rCameraName, COutput_GetCameraResolution* outptr ) -{ - Output_GetCameraResolution outpt = ((Client*) client)->GetCameraResolution( i_rCameraName ); - outptr->Result = outpt.Result; - outptr->ResolutionX = outpt.ResolutionX; - outptr->ResolutionY = outpt.ResolutionY; -} - -void Client_GetIsVideoCamera( CClient* client, CString i_rCameraName, COutput_GetIsVideoCamera* outptr ) -{ - Output_GetIsVideoCamera outpt = ((Client*) client)->GetIsVideoCamera( (String)i_rCameraName ); - outptr->Result = outpt.Result; - outptr->IsVideoCamera = outpt.IsVideoCamera; -} - -void Client_GetCentroidCount( CClient* client, CString i_rCameraName, COutput_GetCentroidCount* outptr ) -{ - Output_GetCentroidCount outpt = ((Client*) client)->GetCentroidCount( (String)i_rCameraName ); - outptr->Result = outpt.Result; - outptr->CentroidCount = outpt.CentroidCount; -} - -void Client_GetCentroidPosition( CClient* client, CString i_rCameraName, unsigned int i_CentroidIndex, COutput_GetCentroidPosition* outptr ) -{ - Output_GetCentroidPosition outpt = ((Client*) client)->GetCentroidPosition( (String)i_rCameraName, i_CentroidIndex ); - outptr->Result = outpt.Result; - outptr->Radius = outpt.Radius; - std::memcpy( outptr->CentroidPosition, outpt.CentroidPosition, sizeof( outptr->CentroidPosition ) ); -} - -void Client_GetCentroidWeight( CClient* client, CString i_rCameraName, unsigned int i_CentroidIndex, COutput_GetCentroidWeight* outptr ) -{ - Output_GetCentroidWeight outpt = ((Client*) client)->GetCentroidWeight( (String)i_rCameraName, i_CentroidIndex ); - outptr->Result = outpt.Result; - outptr->Weight = outpt.Weight; -} - -/* -void Client_GetGreyscaleBlobCount( CClient* client, CString i_rCameraName, COutput_GetGreyscaleBlobCount* outptr ) -{ - Output_GetGreyscaleBlobCount outpt = ((Client*) client)->GetGreyscaleBlobCount( (String) i_rCameraName ); - outptr->Result = outpt.Result; - outptr->BlobCount = outpt.BlobCount; -} - -void Client_GetGreyscaleBlob( CClient* client, CString i_rCameraName, unsigned int i_BlobIndex, COutput_GetGreyscaleBlob* outptr ) -{ - Output_GetGreyscaleBlob outpt = ((Client*) client)->GetGreyscaleBlob( (String)i_rCameraName, i_BlobIndex ); - outptr->Result = outpt.Result; -} - -CEnum Client_SetCameraFilter( CClient* client, std::vector< unsigned int > i_rCameraIdsForCentroids, std::vector< unsigned int > i_rCameraIdsForBlobs ) -{ - Output_SetCameraFilter outpt; - - return ((Client*) client)->SetCameraFilter( i_rCameraIdsForCentroids, i_rCameraIdsForBlobs ) ); - return outpt; -} -*/ - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/CClient.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/CClient.h deleted file mode 100644 index 419d12828..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/CClient.h +++ /dev/null @@ -1,307 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once -/** -* @file CClient.h -* @brief The C implementation of the Vicon DataStream SDK -*/ - -#ifdef WIN32 - -#ifdef _EXPORTING -#define CDLL_EXPORT __declspec(dllexport) -#else -#define CDLL_EXPORT __declspec(dllimport) -#endif // _EXPORTING - -#elif defined( __GNUC__ ) - -#if __GNUC__ < 4 -#error gcc 4 is required. -#endif -#define CDLL_EXPORT __attribute__((visibility("default"))) - -#else - -#define CDLL_EXPORT - -#endif - - - -#ifdef __cplusplus -extern "C" { -#endif - -#include "CTypeDefs.h" - - /** Construction. - * You can create many instances of the Vicon DataStream Client which can connect to multiple Vicon DataStream Servers. - * .NET is object oriented, so use the class constructor. - * - * Because objects are lazily garbage collected, your instance may outlive the last reference to it for some time. - * - * If the instance is pre-fetching frame data for you, then it can still use CPU and network bandwidth. - * - * Consider explicitly disconnecting prior to destruction. - * - * ViconDataStreamSDK.DotNET.Client pHeapClient = new ViconDataStreamSDK.DotNET.Client(); - * Output_SomeFunction Output = pHeapClient.SomeFunction(InputParam); - * - * // Signal to the garbage collector that it can clean up - * pHeapClient.Disconnect(); - * pHeapClient = null; - */ - CDLL_EXPORT CClient* Client_Create(void); - -/** Destruction. -* You can create many instances of the Vicon DataStream Client which can connect to multiple Vicon DataStream Servers. -* .NET is object oriented, so use the class constructor. -* -* Because objects are lazily garbage collected, your instance may outlive the last reference to it for some time. -* -* If the instance is pre-fetching frame data for you, then it can still use CPU and network bandwidth. -* -* Consider explicitly disconnecting prior to destruction. -* -* ViconDataStreamSDK.DotNET.Client pHeapClient = new ViconDataStreamSDK.DotNET.Client(); -* Output_SomeFunction Output = pHeapClient.SomeFunction(InputParam); -* -* // Signal to the garbage collector that it can clean up -* pHeapClient.Disconnect(); -* pHeapClient = null; -*/ -CDLL_EXPORT void Client_Destroy(CClient* client); - -CDLL_EXPORT void Client_GetVersion( CClient* client, COutput_GetVersion* outptr); - - -CDLL_EXPORT CBool Client_Connect(CClient* client, CString HostName ); -CDLL_EXPORT CEnum Client_ConnectToMulticast(CClient* client, CString LocalIP, CString MulticastIP ); -CDLL_EXPORT CEnum Client_Disconnect(CClient* client); -CDLL_EXPORT CBool Client_IsConnected(CClient* client); - -CDLL_EXPORT CEnum Client_StartTransmittingMulticast(CClient* client, CString ServerIP, - CString MulticastIP ); - -CDLL_EXPORT CEnum Client_StopTransmittingMulticast(CClient* client); - -CDLL_EXPORT CEnum Client_EnableSegmentData(CClient* client); -CDLL_EXPORT CEnum Client_EnableMarkerData(CClient* client); -CDLL_EXPORT CEnum Client_EnableUnlabeledMarkerData(CClient* client); -CDLL_EXPORT CEnum Client_EnableDeviceData(CClient* client); - -CDLL_EXPORT CEnum Client_DisableSegmentData(CClient* client); -CDLL_EXPORT CEnum Client_DisableMarkerData(CClient* client); -CDLL_EXPORT CEnum Client_DisableUnlabeledMarkerData(CClient* client); -CDLL_EXPORT CEnum Client_DisableDeviceData(CClient* client); - -CDLL_EXPORT CBool Client_IsSegmentDataEnabled(CClient* client); -CDLL_EXPORT CBool Client_IsMarkerDataEnabled(CClient* client); -CDLL_EXPORT CBool Client_IsUnlabeledMarkerDataEnabled(CClient* client); -CDLL_EXPORT CBool Client_IsDeviceDataEnabled(CClient* client); - -CDLL_EXPORT CEnum Client_SetStreamMode(CClient* client, CEnum Mode ); - -CDLL_EXPORT CEnum Client_SetApexDeviceFeedback(CClient* client, CString i_rDeviceName, CBool i_bOn ); - -CDLL_EXPORT CEnum Client_SetAxisMapping(CClient* client, CEnum XAxis, CEnum YAxis, CEnum ZAxis ); -CDLL_EXPORT void Client_GetAxisMapping(CClient* client, COutput_GetAxisMapping* outptr); - -CDLL_EXPORT CEnum Client_GetFrame(CClient* client); -CDLL_EXPORT void Client_GetFrameNumber(CClient* client, COutput_GetFrameNumber* outptr); - -CDLL_EXPORT void Client_GetTimecode(CClient* client, COutput_GetTimecode* outptr); - -CDLL_EXPORT void Client_GetFrameRate(CClient* client, COutput_GetFrameRate* outptr); - -CDLL_EXPORT void Client_GetLatencySampleCount(CClient* client, COutput_GetLatencySampleCount* outptr); -CDLL_EXPORT CEnum Client_GetLatencySampleName(CClient* client, unsigned int LatencySampleIndex, - int sizeOfBuffer, char* outstr ); -CDLL_EXPORT void Client_GetLatencySampleValue(CClient* client, CString LatencySampleName, COutput_GetLatencySampleValue* outptr ); -CDLL_EXPORT void Client_GetLatencyTotal(CClient* client, COutput_GetLatencyTotal* outptr); - -CDLL_EXPORT void Client_GetSubjectCount(CClient* client, COutput_GetSubjectCount* outptr); -CDLL_EXPORT CEnum Client_GetSubjectName(CClient* client, unsigned int SubjectIndex, int sizeOfBuffer, char* outstr ); - -CDLL_EXPORT CEnum Client_GetSubjectRootSegmentName(CClient* client, CString SubjectName, int sizeOfBuffer, char* outstr ); - -CDLL_EXPORT void Client_GetSegmentCount( CClient* client,CString SubjectName,COutput_GetSegmentCount* outptr ); - -CDLL_EXPORT CEnum Client_GetSegmentName(CClient* client, CString SubjectName, - unsigned int SegmentIndex, int sizeOfBuffer, char* outstr ); - -CDLL_EXPORT void Client_GetSegmentChildCount(CClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentChildCount* outptr ); - -CDLL_EXPORT CEnum Client_GetSegmentChildName(CClient* client, CString SubjectName, - CString SegmentName, - unsigned int SegmentIndex, int sizeOfBuffer, char* outstr ); - -CDLL_EXPORT CEnum Client_GetSegmentParentName(CClient* client, CString SubjectName, - CString SegmentName, int sizeOfBuffer, char* outstr ); - -CDLL_EXPORT void Client_GetSegmentStaticTranslation(CClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentStaticTranslation* outptr ); - -CDLL_EXPORT void Client_GetSegmentStaticRotationHelical(CClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentStaticRotationHelical* outptr ); - -CDLL_EXPORT void Client_GetSegmentStaticRotationMatrix(CClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentStaticRotationMatrix* outptr ); - -CDLL_EXPORT void Client_GetSegmentStaticRotationQuaternion(CClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentStaticRotationQuaternion* outptr ); - -CDLL_EXPORT void Client_GetSegmentStaticRotationEulerXYZ(CClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentStaticRotationEulerXYZ* outptr ); - -CDLL_EXPORT void Client_GetSegmentGlobalTranslation(CClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentGlobalTranslation* outptr ); - -CDLL_EXPORT void Client_GetSegmentGlobalRotationHelical(CClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentGlobalRotationHelical* outptr ); - -CDLL_EXPORT void Client_GetSegmentGlobalRotationMatrix(CClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentGlobalRotationMatrix* outptr ); - -CDLL_EXPORT void Client_GetSegmentGlobalRotationQuaternion(CClient* client, CString SubjectName, CString SegmentName, - COutput_GetSegmentGlobalRotationQuaternion* outptr); - -CDLL_EXPORT void Client_GetSegmentGlobalRotationEulerXYZ(CClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentGlobalRotationEulerXYZ* outptr ); - -CDLL_EXPORT void Client_GetSegmentLocalTranslation(CClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentLocalTranslation* outptr ); - -CDLL_EXPORT void Client_GetSegmentLocalRotationHelical(CClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentLocalRotationHelical* outptr ); - -CDLL_EXPORT void Client_GetSegmentLocalRotationMatrix(CClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentLocalRotationMatrix* outptr ); - -CDLL_EXPORT void Client_GetSegmentLocalRotationQuaternion(CClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentLocalRotationQuaternion* outptr ); - -CDLL_EXPORT void Client_GetSegmentLocalRotationEulerXYZ(CClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentLocalRotationEulerXYZ* outptr ); - -CDLL_EXPORT void Client_GetMarkerCount(CClient* client, CString SubjectName, COutput_GetMarkerCount* outptr ); - -CDLL_EXPORT CEnum Client_GetMarkerName(CClient* client, CString SubjectName, - unsigned int MarkerIndex, int sizeOfBuffer, char* outstr ); - -CDLL_EXPORT CEnum Client_GetMarkerParentName(CClient* client, CString SubjectName, - CString MarkerName, int sizeOfBuffer, char* outstr ); - -CDLL_EXPORT void Client_GetMarkerGlobalTranslation(CClient* client, CString SubjectName, - CString MarkerName, COutput_GetMarkerGlobalTranslation* outptr ); - -CDLL_EXPORT void Client_GetUnlabeledMarkerCount(CClient* client, COutput_GetUnlabeledMarkerCount* outptr); - -CDLL_EXPORT void Client_GetUnlabeledMarkerGlobalTranslation(CClient* client, unsigned int MarkerIndex, - COutput_GetUnlabeledMarkerGlobalTranslation* outptr ); - -CDLL_EXPORT void Client_GetDeviceCount(CClient* client, COutput_GetDeviceCount* outptr); -CDLL_EXPORT CEnum Client_GetDeviceName(CClient* client, unsigned int DeviceIndex, - int sizeOfBuffer, char* outstr, CEnum* DeviceType ); - -CDLL_EXPORT void Client_GetDeviceOutputCount(CClient* client, CString DeviceName, COutput_GetDeviceOutputCount* outptr ); - -CDLL_EXPORT CEnum Client_GetDeviceOutputName(CClient* client, CString DeviceName, - unsigned int DeviceOutputIndex, int sizeOfBuffer, char* outstr, CEnum* DeviceOutputUnit ); - -CDLL_EXPORT void Client_GetDeviceOutputValue(CClient* client, CString DeviceName, - CString DeviceOutputName, COutput_GetDeviceOutputValue* outptr ); - -CDLL_EXPORT void Client_GetDeviceOutputSubsamples(CClient* client, CString DeviceName, - CString DeviceOutputName, COutput_GetDeviceOutputSubsamples* outptr ); - -CDLL_EXPORT void Client_GetDeviceOutputValueForSubsample(CClient* client, CString DeviceName, - CString DeviceOutputName, - unsigned int Subsample, - COutput_GetDeviceOutputValue* outptr); - -CDLL_EXPORT void Client_GetForcePlateCount(CClient* client, COutput_GetForcePlateCount* outptr); - -CDLL_EXPORT void Client_GetGlobalForceVector(CClient* client, unsigned int ForcePlateIndex, COutput_GetGlobalForceVector* outptr ); -CDLL_EXPORT void Client_GetGlobalMomentVector(CClient* client, unsigned int ForcePlateIndex, COutput_GetGlobalMomentVector* outptr ); -CDLL_EXPORT void Client_GetGlobalCentreOfPressure(CClient* client, unsigned int ForcePlateIndex, COutput_GetGlobalCentreOfPressure* outptr ); - -CDLL_EXPORT void Client_GetForcePlateSubsamples(CClient* client, unsigned int ForcePlateIndex, COutput_GetForcePlateSubsamples* outptr ); - -CDLL_EXPORT void Client_GetGlobalForceVectorForSubsample(CClient* client, unsigned int ForcePlateIndex, unsigned int Subsample, - COutput_GetGlobalForceVector* outptr); -CDLL_EXPORT void Client_GetGlobalMomentVectorForSubsample(CClient* client, unsigned int ForcePlateIndex, unsigned int Subsample, - COutput_GetGlobalMomentVector* outptr); -CDLL_EXPORT void Client_GetGlobalCentreOfPressureForSubsample(CClient* client, unsigned int ForcePlateIndex, unsigned int Subsample, - COutput_GetGlobalCentreOfPressure* outptr); - -CDLL_EXPORT void Client_GetEyeTrackerCount(CClient* client, COutput_GetEyeTrackerCount* outptr); - -CDLL_EXPORT void Client_GetEyeTrackerGlobalPosition(CClient* client, unsigned int EyeTrackerIndex, - COutput_GetEyeTrackerGlobalPosition* outptr); -CDLL_EXPORT void Client_GetEyeTrackerGlobalGazeVector(CClient* client, unsigned int EyeTrackerIndex, - COutput_GetEyeTrackerGlobalGazeVector* outptr); - -CDLL_EXPORT CEnum Client_EnableMarkerRayData( CClient* client ); -CDLL_EXPORT CEnum Client_EnableCentroidData( CClient* client ); -CDLL_EXPORT CEnum Client_EnableGreyscaleData( CClient* client ); -CDLL_EXPORT CEnum Client_EnableDebugData( CClient* client ); -CDLL_EXPORT CEnum Client_DisableMarkerRayData( CClient* client ); -CDLL_EXPORT CEnum Client_DisableCentroidData( CClient* client ); -CDLL_EXPORT CEnum Client_DisableGreyscaleData( CClient* client ); -CDLL_EXPORT CEnum Client_DisableDebugData( CClient* client ); -CDLL_EXPORT CBool Client_IsMarkerRayDataEnabled( CClient* client ); -CDLL_EXPORT CBool Client_IsCentroidDataEnabled( CClient* client ); -CDLL_EXPORT CBool Client_IsGreyscaleDataEnabled( CClient* client ); -CDLL_EXPORT CBool Client_IsDebugDataEnabled( CClient* client ); - -CDLL_EXPORT void Client_GetServerOrientation( CClient* client, COutput_GetServerOrientation* outptr ); -CDLL_EXPORT void Client_GetHardwareFrameNumber( CClient* client, COutput_GetHardwareFrameNumber* outptr ); -CDLL_EXPORT void Client_GetFrameRateCount( CClient* client, COutput_GetFrameRateCount* outptr ); -CDLL_EXPORT CEnum Client_GetFrameRateName( CClient* client, unsigned int FrameRateIndex, int sizeOfBuffer, char* outstr ); -CDLL_EXPORT void Client_GetFrameRateValue( CClient* client, CString FrameRateName, COutput_GetFrameRateValue* outptr ); -CDLL_EXPORT void Client_GetObjectQuality( CClient* client, CString ObjectName, COutput_GetObjectQuality* outptr ); -CDLL_EXPORT void Client_GetMarkerRayContributionCount( CClient* client, CString SubjectName, CString MarkerName, COutput_GetMarkerRayContributionCount* outptr ); -CDLL_EXPORT void Client_GetMarkerRayContribution( CClient* client, CString SubjectName, CString MarkerName, unsigned int MarkerRayContributionIndex, COutput_GetMarkerRayContribution* outptr ); -CDLL_EXPORT void Client_GetLabeledMarkerCount( CClient* client, COutput_GetLabeledMarkerCount* outptr ); -CDLL_EXPORT void Client_GetLabeledMarkerGlobalTranslation( CClient* client, unsigned int MarkerIndex, COutput_GetLabeledMarkerGlobalTranslation* outptr ); -CDLL_EXPORT void Client_GetCameraCount( CClient* client, COutput_GetCameraCount* outptr ); -CDLL_EXPORT CEnum Client_GetCameraName( CClient* client, unsigned int i_CameraIndex, int sizeOfBuffer, char* outstr ); -CDLL_EXPORT void Client_GetCameraId( CClient* client, CString i_rCameraName, COutput_GetCameraId* outptr ); -CDLL_EXPORT void Client_GetCameraUserId( CClient* client, CString i_rCameraName, COutput_GetCameraUserId* outptr ); -CDLL_EXPORT CEnum Client_GetCameraType( CClient* client, CString i_rCameraName, int sizeOfBuffer, char* outstr ); -CDLL_EXPORT CEnum Client_GetCameraDisplayName( CClient* client, CString i_rCameraName, int sizeOfBuffer, char* outstr ); -CDLL_EXPORT void Client_GetCameraResolution( CClient* client, CString i_rCameraName, COutput_GetCameraResolution* outptr ); -CDLL_EXPORT void Client_GetIsVideoCamera( CClient* client, CString i_rCameraName, COutput_GetIsVideoCamera* outptr ); -CDLL_EXPORT void Client_GetCentroidCount( CClient* client, CString i_rCameraName, COutput_GetCentroidCount* outptr ); -CDLL_EXPORT void Client_GetCentroidPosition( CClient* client, CString i_rCameraName, unsigned int i_CentroidIndex, COutput_GetCentroidPosition* outptr ); -CDLL_EXPORT void Client_GetCentroidWeight( CClient* client, CString i_rCameraName, unsigned int i_CentroidIndex, COutput_GetCentroidWeight* outptr ); - - -#ifdef __cplusplus -} -#endif diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/CRetimingClient.cpp b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/CRetimingClient.cpp deleted file mode 100644 index 19c740171..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/CRetimingClient.cpp +++ /dev/null @@ -1,364 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#include "CRetimingClient.h" -#include - -#include "CRetimingClient.h" -#include -#include - -using namespace ViconDataStreamSDK::CPP; - - -#ifdef _WIN32 -#define snprintf(str,size,format,arg) _snprintf_s(str,size,_TRUNCATE,format,arg) -#endif - -CRetimingClient* RetimingClient_Create(void) -{ - RetimingClient* retimer = new RetimingClient(); - return retimer; -} - -void RetimingClient_Destroy( CRetimingClient* client ) -{ - if( client ) - { - delete ( (CRetimingClient*)client ); - } -} - -void RetimingClient_GetVersion(CRetimingClient* client, COutput_GetVersion* outptr) -{ - const Output_GetVersion& outp = ((RetimingClient*)client)->GetVersion(); - outptr->Major = outp.Major; outptr->Minor = outp.Minor; outptr->Point = outp.Point; -} - -CEnum RetimingClient_Connect(CRetimingClient* client, CString HostName) -{ - return ((RetimingClient*)client)->Connect(String(HostName)).Result; -} - -CEnum RetimingClient_ConnectAndStart(CRetimingClient* client, CString HostName, double FrameRate ) -{ - return ((RetimingClient*)client)->Connect(String(HostName), FrameRate ).Result; -} - -CEnum RetimingClient_Disconnect(CRetimingClient* client) -{ - return ((RetimingClient*)client)->Disconnect().Result; -} - -CBool RetimingClient_IsConnected(CRetimingClient* client) -{ - return ((RetimingClient*)client)->IsConnected().Connected; -} - -CEnum RetimingClient_SetAxisMapping(CRetimingClient* client, CEnum XAxis, CEnum YAxis, CEnum ZAxis) -{ - return ((RetimingClient*)client)->SetAxisMapping((Direction::Enum) XAxis, (Direction::Enum) YAxis, - (Direction::Enum) ZAxis).Result; -} - -void RetimingClient_GetAxisMapping(CRetimingClient* client, COutput_GetAxisMapping* outptr) -{ - const Output_GetAxisMapping& outp = ((RetimingClient*)client)->GetAxisMapping(); - - outptr->XAxis = outp.XAxis; - outptr->YAxis = outp.YAxis; - outptr->ZAxis = outp.ZAxis; -} - -CEnum RetimingClient_UpdateFrame(CRetimingClient* client ) -{ - return ((RetimingClient*)client)->UpdateFrame( 0.0 ).Result; -} - -CEnum RetimingClient_UpdateFrameOffset(CRetimingClient* client, double i_Offset) -{ - return ((RetimingClient*)client)->UpdateFrame(i_Offset).Result; -} - -CEnum RetimingClient_WaitForFrame(CRetimingClient* client) -{ - return ((RetimingClient*)client)->WaitForFrame().Result; -} - -void RetimingClient_GetSubjectCount(CRetimingClient* client, COutput_GetSubjectCount* outptr) -{ - const Output_GetSubjectCount& outp = ((RetimingClient*)client)->GetSubjectCount(); - - outptr->Result = outp.Result; - outptr->SubjectCount = outp.SubjectCount; -} - -CEnum RetimingClient_GetSubjectName(CRetimingClient* client, unsigned int SubjectIndex, int sizeOfBuffer, char* outstr) -{ - const Output_GetSubjectName& outp = ((RetimingClient*)client)->GetSubjectName(SubjectIndex); - - snprintf(outstr, sizeOfBuffer, "%s", std::string(outp.SubjectName).c_str()); - return outp.Result; -} - -CEnum RetimingClient_GetSubjectRootSegmentName(CRetimingClient* client, CString SubjectName, int sizeOfBuffer, char* outstr) -{ - const Output_GetSubjectRootSegmentName& outp = ((RetimingClient*)client)->GetSubjectRootSegmentName(String(SubjectName)); - - snprintf(outstr, sizeOfBuffer, "%s", std::string(outp.SegmentName).c_str()); - return outp.Result; -} - -void RetimingClient_GetSegmentCount(CRetimingClient* client, CString SubjectName, COutput_GetSegmentCount* outptr) -{ - const Output_GetSegmentCount& outp = ((RetimingClient*)client)->GetSegmentCount(String(SubjectName)); - - outptr->Result = outp.Result; - outptr->SegmentCount = outp.SegmentCount; -} - -CEnum RetimingClient_GetSegmentName(CRetimingClient* client, CString SubjectName, - unsigned int SegmentIndex, int sizeOfBuffer, char* outstr) -{ - const Output_GetSegmentName& outp = ((RetimingClient*)client)->GetSegmentName(String(SubjectName), SegmentIndex); - - snprintf(outstr, sizeOfBuffer, "%s", std::string(outp.SegmentName).c_str()); - return outp.Result; -} - -void RetimingClient_GetSegmentChildCount(CRetimingClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentChildCount* outptr) -{ - const Output_GetSegmentChildCount& outp = ((RetimingClient*)client)->GetSegmentChildCount(String(SubjectName), - String(SegmentName)); - - outptr->Result = outp.Result; - outptr->SegmentCount = outp.SegmentCount; -} - -CEnum RetimingClient_GetSegmentChildName(CRetimingClient* client, CString SubjectName, - CString SegmentName, - unsigned int SegmentIndex, - int sizeOfBuffer, char* outstr) -{ - const Output_GetSegmentChildName& outp = ((RetimingClient*)client)->GetSegmentChildName(String(SubjectName), - String(SegmentName), SegmentIndex); - - snprintf(outstr, sizeOfBuffer, "%s", std::string(outp.SegmentName).c_str()); - return outp.Result; -} - -CEnum RetimingClient_GetSegmentParentName(CRetimingClient* client, CString SubjectName, - CString SegmentName, int sizeOfBuffer, char* outstr) -{ - const Output_GetSegmentParentName& outp = ((RetimingClient*)client)->GetSegmentParentName(String(SubjectName), - String(SegmentName)); - - snprintf(outstr, sizeOfBuffer, "%s", std::string(outp.SegmentName).c_str()); - return outp.Result; -} - -void RetimingClient_GetSegmentStaticTranslation(CRetimingClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentStaticTranslation* outptr) -{ - const Output_GetSegmentStaticTranslation& outp = ((RetimingClient*)client)->GetSegmentStaticTranslation(String(SubjectName), - String(SegmentName)); - - outptr->Result = outp.Result; - std::memcpy(outptr->Translation, outp.Translation, sizeof(outptr->Translation)); -} - -void RetimingClient_GetSegmentStaticRotationHelical(CRetimingClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentStaticRotationHelical* outptr) -{ - const Output_GetSegmentStaticRotationHelical& outp = ((RetimingClient*)client)->GetSegmentStaticRotationHelical( - String(SubjectName), String(SegmentName)); - - outptr->Result = outp.Result; - std::memcpy(outptr->Rotation, outp.Rotation, sizeof(outptr->Rotation)); -} - -void RetimingClient_GetSegmentStaticRotationMatrix(CRetimingClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentStaticRotationMatrix* outptr) -{ - const Output_GetSegmentStaticRotationMatrix& outp = ((RetimingClient*)client)->GetSegmentStaticRotationMatrix( - String(SubjectName), String(SegmentName)); - - outptr->Result = outp.Result; - std::memcpy(outptr->Rotation, outp.Rotation, sizeof(outptr->Rotation)); -} - -void RetimingClient_GetSegmentStaticRotationQuaternion(CRetimingClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentStaticRotationQuaternion* outptr) -{ - const Output_GetSegmentStaticRotationQuaternion& outp = ((RetimingClient*)client)->GetSegmentStaticRotationQuaternion( - String(SubjectName), String(SegmentName)); - - outptr->Result = outp.Result; - std::memcpy(outptr->Rotation, outp.Rotation, sizeof(outptr->Rotation)); -} - -void RetimingClient_GetSegmentStaticRotationEulerXYZ(CRetimingClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentStaticRotationEulerXYZ* outptr) -{ - const Output_GetSegmentStaticRotationEulerXYZ& outp = ((RetimingClient*)client)->GetSegmentStaticRotationEulerXYZ( - String(SubjectName), String(SegmentName)); - - outptr->Result = outp.Result; - std::memcpy(outptr->Rotation, outp.Rotation, sizeof(outptr->Rotation)); -} - -void RetimingClient_GetSegmentGlobalTranslation(CRetimingClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentGlobalTranslation* outptr) -{ - const Output_GetSegmentGlobalTranslation& outp = ((RetimingClient*)client)->GetSegmentGlobalTranslation( - String(SubjectName), String(SegmentName)); - - outptr->Result = outp.Result; - std::memcpy(outptr->Translation, outp.Translation, sizeof(outptr->Translation)); - outptr->Occluded = outp.Occluded; -} - -void RetimingClient_GetSegmentGlobalRotationHelical(CRetimingClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentGlobalRotationHelical* outptr) -{ - const Output_GetSegmentGlobalRotationHelical& outp = ((RetimingClient*)client)->GetSegmentGlobalRotationHelical( - String(SubjectName), String(SegmentName)); - - outptr->Result = outp.Result; - std::memcpy(outptr->Rotation, outp.Rotation, sizeof(outptr->Rotation)); - outptr->Occluded = outp.Occluded; -} - -void RetimingClient_GetSegmentGlobalRotationMatrix(CRetimingClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentGlobalRotationMatrix* outptr) -{ - const Output_GetSegmentGlobalRotationMatrix& outp = ((RetimingClient*)client)->GetSegmentGlobalRotationMatrix( - String(SubjectName), String(SegmentName)); - - outptr->Result = outp.Result; - std::memcpy(outptr->Rotation, outp.Rotation, sizeof(outptr->Rotation)); - outptr->Occluded = outp.Occluded; -} - -void RetimingClient_GetSegmentGlobalRotationQuaternion(CRetimingClient* client, CString SubjectName, CString SegmentName, - COutput_GetSegmentGlobalRotationQuaternion* outptr) -{ - const Output_GetSegmentGlobalRotationQuaternion& outp = ((RetimingClient*)client)->GetSegmentGlobalRotationQuaternion( - String(SubjectName), String(SegmentName)); - - outptr->Result = outp.Result; - - std::memcpy(outptr->Rotation, outp.Rotation, sizeof(outptr->Rotation)); - - outptr->Occluded = outp.Occluded; -} - - -void RetimingClient_GetSegmentGlobalRotationEulerXYZ(CRetimingClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentGlobalRotationEulerXYZ* outptr) -{ - const Output_GetSegmentGlobalRotationEulerXYZ& outp = ((RetimingClient*)client)->GetSegmentGlobalRotationEulerXYZ( - String(SubjectName), String(SegmentName)); - - outptr->Result = outp.Result; - std::memcpy(outptr->Rotation, outp.Rotation, sizeof(outptr->Rotation)); - outptr->Occluded = outp.Occluded; -} - -void RetimingClient_GetSegmentLocalTranslation(CRetimingClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentLocalTranslation* outptr) -{ - const Output_GetSegmentLocalTranslation& outp = ((RetimingClient*)client)->GetSegmentLocalTranslation( - String(SubjectName), String(SegmentName)); - - outptr->Result = outp.Result; - std::memcpy(outptr->Translation, outp.Translation, sizeof(outptr->Translation)); - outptr->Occluded = outp.Occluded; -} - -void RetimingClient_GetSegmentLocalRotationHelical(CRetimingClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentLocalRotationHelical* outptr) -{ - const Output_GetSegmentLocalRotationHelical& outp = ((RetimingClient*)client)->GetSegmentLocalRotationHelical( - String(SubjectName), String(SegmentName)); - - outptr->Result = outp.Result; - std::memcpy(outptr->Rotation, outp.Rotation, sizeof(outptr->Rotation)); - outptr->Occluded = outp.Occluded; -} - -void RetimingClient_GetSegmentLocalRotationMatrix(CRetimingClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentLocalRotationMatrix* outptr) -{ - const Output_GetSegmentLocalRotationMatrix& outp = ((RetimingClient*)client)->GetSegmentLocalRotationMatrix( - String(SubjectName), String(SegmentName)); - - outptr->Result = outp.Result; - std::memcpy(outptr->Rotation, outp.Rotation, sizeof(outptr->Rotation)); - outptr->Occluded = outp.Occluded; -} - -void RetimingClient_GetSegmentLocalRotationQuaternion(CRetimingClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentLocalRotationQuaternion* outptr) -{ - const Output_GetSegmentLocalRotationQuaternion& outp = ((RetimingClient*)client)->GetSegmentLocalRotationQuaternion( - String(SubjectName), String(SegmentName)); - - outptr->Result = outp.Result; - std::memcpy(outptr->Rotation, outp.Rotation, sizeof(outptr->Rotation)); - outptr->Occluded = outp.Occluded; -} - -void RetimingClient_GetSegmentLocalRotationEulerXYZ(CRetimingClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentLocalRotationEulerXYZ* outptr) -{ - const Output_GetSegmentLocalRotationEulerXYZ& outp = ((RetimingClient*)client)->GetSegmentLocalRotationEulerXYZ( - String(SubjectName), String(SegmentName)); - - outptr->Result = outp.Result; - std::memcpy(outptr->Rotation, outp.Rotation, sizeof(outptr->Rotation)); - outptr->Occluded = outp.Occluded; -} - - -void RetimingClient_SetOutputLatency( CRetimingClient* client, CReal i_OutputLatency ) -{ - ((RetimingClient*)client)->SetOutputLatency( i_OutputLatency ); -} - -CReal RetimingClient_OutputLatency( CRetimingClient* client ) -{ - return ((RetimingClient*)client)->OutputLatency(); -} - -void RetimingClient_SetMaximumPrediction( CRetimingClient* client, CReal i_MaxPrediction ) -{ - ((RetimingClient*)client)->SetMaximumPrediction( i_MaxPrediction ); -} - -CReal RetimingClient_MaximumPrediction( CRetimingClient* client ) -{ - return ((RetimingClient*)client)->MaximumPrediction(); -} diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/CRetimingClient.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/CRetimingClient.h deleted file mode 100644 index 5b0e3373f..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/CRetimingClient.h +++ /dev/null @@ -1,147 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -#ifdef WIN32 - -#ifdef _EXPORTING -#define CDLL_EXPORT __declspec(dllexport) -#else -#define CDLL_EXPORT __declspec(dllimport) -#endif // _EXPORTING - -#elif defined( __GNUC__ ) - -#if __GNUC__ < 4 -#error gcc 4 is required. -#endif -#define CDLL_EXPORT __attribute__((visibility("default"))) - -#else - -#define CDLL_EXPORT - -#endif - - - -#ifdef __cplusplus -extern "C" { -#endif - -#include "CTypeDefs.h" - -CDLL_EXPORT CRetimingClient* RetimingClient_Create(void); -CDLL_EXPORT void RetimingClient_Destroy( CRetimingClient* client ); - -CDLL_EXPORT void RetimingClient_GetVersion(CRetimingClient* client, COutput_GetVersion* outptr); - -CDLL_EXPORT CBool RetimingClient_Connect(CRetimingClient* client, CString HostName); -CDLL_EXPORT CBool RetimingClient_ConnectAndStart(CRetimingClient* client, CString HostName, double FrameRate ); -CDLL_EXPORT CEnum RetimingClient_Disconnect(CRetimingClient* client); -CDLL_EXPORT CEnum RetimingClient_IsConnected(CRetimingClient* client); - -CDLL_EXPORT CEnum RetimingClient_SetAxisMapping(CRetimingClient* client, CEnum XAxis, CEnum YAxis, CEnum ZAxis); -CDLL_EXPORT void RetimingClient_GetAxisMapping(CRetimingClient* client, COutput_GetAxisMapping* outptr); - -CDLL_EXPORT CEnum RetimingClient_UpdateFrame(CRetimingClient* client ); -CDLL_EXPORT CEnum RetimingClient_UpdateFrameOffset(CRetimingClient* client, double i_Offset); -CDLL_EXPORT CEnum RetimingClient_WaitForFrame(CRetimingClient* client); - -CDLL_EXPORT void RetimingClient_GetSubjectCount(CRetimingClient* client, COutput_GetSubjectCount* outptr); -CDLL_EXPORT CEnum RetimingClient_GetSubjectName(CRetimingClient* client, unsigned int SubjectIndex, int sizeOfBuffer, char* outstr); - -CDLL_EXPORT CEnum RetimingClient_GetSubjectRootSegmentName(CRetimingClient* client, CString SubjectName, int sizeOfBuffer, char* outstr); - -CDLL_EXPORT void RetimingClient_GetSegmentCount(CRetimingClient* client, CString SubjectName, COutput_GetSegmentCount* outptr); - -CDLL_EXPORT CEnum RetimingClient_GetSegmentName(CRetimingClient* client, CString SubjectName, - unsigned int SegmentIndex, int sizeOfBuffer, char* outstr); - -CDLL_EXPORT void RetimingClient_GetSegmentChildCount(CRetimingClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentChildCount* outptr); - -CDLL_EXPORT CEnum RetimingClient_GetSegmentChildName(CRetimingClient* client, CString SubjectName, - CString SegmentName, - unsigned int SegmentIndex, int sizeOfBuffer, char* outstr); - -CDLL_EXPORT CEnum RetimingClient_GetSegmentParentName(CRetimingClient* client, CString SubjectName, - CString SegmentName, int sizeOfBuffer, char* outstr); - -CDLL_EXPORT void RetimingClient_GetSegmentStaticTranslation(CRetimingClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentStaticTranslation* outptr); - -CDLL_EXPORT void RetimingClient_GetSegmentStaticRotationHelical(CRetimingClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentStaticRotationHelical* outptr); - -CDLL_EXPORT void RetimingClient_GetSegmentStaticRotationMatrix(CRetimingClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentStaticRotationMatrix* outptr); - -CDLL_EXPORT void RetimingClient_GetSegmentStaticRotationQuaternion(CRetimingClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentStaticRotationQuaternion* outptr); - -CDLL_EXPORT void RetimingClient_GetSegmentStaticRotationEulerXYZ(CRetimingClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentStaticRotationEulerXYZ* outptr); - -CDLL_EXPORT void RetimingClient_GetSegmentGlobalTranslation(CRetimingClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentGlobalTranslation* outptr); - -CDLL_EXPORT void RetimingClient_GetSegmentGlobalRotationHelical(CRetimingClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentGlobalRotationHelical* outptr); - -CDLL_EXPORT void RetimingClient_GetSegmentGlobalRotationMatrix(CRetimingClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentGlobalRotationMatrix* outptr); - -CDLL_EXPORT void RetimingClient_GetSegmentGlobalRotationQuaternion(CRetimingClient* client, CString SubjectName, CString SegmentName, - COutput_GetSegmentGlobalRotationQuaternion* outptr); - -CDLL_EXPORT void RetimingClient_GetSegmentGlobalRotationEulerXYZ(CRetimingClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentGlobalRotationEulerXYZ* outptr); - -CDLL_EXPORT void RetimingClient_GetSegmentLocalTranslation(CRetimingClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentLocalTranslation* outptr); - -CDLL_EXPORT void RetimingClient_GetSegmentLocalRotationHelical(CRetimingClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentLocalRotationHelical* outptr); - -CDLL_EXPORT void RetimingClient_GetSegmentLocalRotationMatrix(CRetimingClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentLocalRotationMatrix* outptr); - -CDLL_EXPORT void RetimingClient_GetSegmentLocalRotationQuaternion(CRetimingClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentLocalRotationQuaternion* outptr); - -CDLL_EXPORT void RetimingClient_GetSegmentLocalRotationEulerXYZ(CRetimingClient* client, CString SubjectName, - CString SegmentName, COutput_GetSegmentLocalRotationEulerXYZ* outptr); - - -CDLL_EXPORT void RetimingClient_SetOutputLatency( CRetimingClient* client, CReal i_OutputLatency ); -CDLL_EXPORT CReal RetimingClient_OutputLatency( CRetimingClient* client ); - -CDLL_EXPORT void RetimingClient_SetMaximumPrediction( CRetimingClient* client, CReal i_MaxPrediction ); -CDLL_EXPORT CReal RetimingClient_MaximumPrediction( CRetimingClient* client ); - -#ifdef __cplusplus -} -#endif diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/CTypeDefs.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/CTypeDefs.h deleted file mode 100644 index a8487af26..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/CTypeDefs.h +++ /dev/null @@ -1,748 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -typedef double CReal; -typedef void CClient; -typedef void CRetimingClient; -typedef const char* CString; -typedef int CBool; -typedef int CEnum; - -static const int CSuccess = 2; - -#pragma pack(push, 1) - -/** @private */ -typedef struct COutput_GetVersion -{ - unsigned int Major; - unsigned int Minor; - unsigned int Point; -} COutput_GetVersion; - -/** @private */ -typedef struct COutput_Connect -{ - CEnum Result; -} COutput_Connect; - -/** @private */ -typedef struct COutput_ConnectToMulticast -{ - CEnum Result; -} COutput_ConnectToMulticast; - -/** @private */ -typedef struct COutput_Disconnect -{ - CEnum Result; -} COutput_Disconnect; - -/** @private */ -typedef struct COutput_IsConnected -{ - CBool Connected; -} COutput_IsConnected; - -/** @private */ -typedef struct COutput_StartTransmittingMulticast -{ - CEnum Result; -} COutput_StartTransmittingMulticast; - -/** @private */ -typedef struct COutput_StopTransmittingMulticast -{ - CEnum Result; -} COutput_StopTransmittingMulticast; - -/** @private */ -typedef struct COutput_EnableSegmentData -{ - CEnum Result; -} COutput_EnableSegmentData; - -/** @private */ -typedef struct COutput_EnableMarkerData -{ - CEnum Result; -} COutput_EnableMarkerData; - -/** @private */ -typedef struct COutput_EnableUnlabeledMarkerData -{ - CEnum Result; -} COutput_EnableUnlabeledMarkerData; - -/** @private */ -typedef struct COutput_EnableDeviceData -{ - CEnum Result; -} COutput_EnableDeviceData; - -/** @private */ -typedef struct COutput_DisableSegmentData -{ - CEnum Result; -} COutput_DisableSegmentData; - -/** @private */ -typedef struct COutput_DisableMarkerData -{ - CEnum Result; -} COutput_DisableMarkerData; - -/** @private */ -typedef struct COutput_DisableUnlabeledMarkerData -{ - CEnum Result; -} COutput_DisableUnlabeledMarkerData; - -/** @private */ -typedef struct COutput_DisableDeviceData -{ - CEnum Result; -} COutput_DisableDeviceData; - -/** @private */ -typedef struct COutput_IsSegmentDataEnabled -{ - CBool Enabled; -} COutput_IsSegmentDataEnabled; - -/** @private */ -typedef struct COutput_IsMarkerDataEnabled -{ - CBool Enabled; -} COutput_IsMarkerDataEnabled; - -/** @private */ -typedef struct COutput_IsUnlabeledMarkerDataEnabled -{ - CBool Enabled; -} COutput_IsUnlabeledMarkerDataEnabled; - -/** @private */ -typedef struct COutput_IsDeviceDataEnabled -{ - CBool Enabled; -} COutput_IsDeviceDataEnabled; - -/** @private */ -typedef struct COutput_SetStreamMode -{ - CEnum Result; -} COutput_SetStreamMode; - -/** @private */ -typedef struct COutput_SetApexDeviceFeedback -{ - CEnum Result; -} COutput_SetApexDeviceFeedback; - -/** @private */ -typedef struct COutput_SetAxisMapping -{ - CEnum Result; -} COutput_SetAxisMapping; - -/** @private */ -typedef struct COutput_GetAxisMapping -{ - CEnum XAxis; - CEnum YAxis; - CEnum ZAxis; -} COutput_GetAxisMapping; - -/** @private */ -typedef struct COutput_GetFrame -{ - CEnum Result; -} COutput_GetFrame; - -/** @private */ -typedef struct COutput_GetFrameNumber -{ - CEnum Result; - unsigned int FrameNumber; -} COutput_GetFrameNumber; - -/** @private */ -typedef struct COutput_GetTimecode -{ - CEnum Result; - unsigned int Hours; - unsigned int Minutes; - unsigned int Seconds; - unsigned int Frames; - unsigned int SubFrame; - CBool FieldFlag; - CEnum Standard; - unsigned int SubFramesPerFrame; - unsigned int UserBits; -} COutput_GetTimecode; - -/** @private */ -typedef struct COutput_GetFrameRate -{ - CEnum Result; - double FrameRateHz; -} COutput_GetFrameRate; - -/** @private */ -typedef struct COutput_GetLatencySampleCount -{ - CEnum Result; - unsigned int Count; -} COutput_GetLatencySampleCount; - -/** @private */ -typedef struct COutput_GetLatencySampleValue -{ - CEnum Result; - double Value; -} COutput_GetLatencySampleValue; - -/** @private */ -typedef struct COutput_GetLatencyTotal -{ - CEnum Result; - double Total; -} COutput_GetLatencyTotal; - -/** @private */ -typedef struct COutput_GetSubjectCount -{ - CEnum Result; - unsigned int SubjectCount; -} COutput_GetSubjectCount; - -/** @private */ -typedef struct COutput_GetSegmentChildCount -{ - CEnum Result; - unsigned int SegmentCount; -} COutput_GetSegmentChildCount; - -/** @private */ -typedef struct COutput_GetSegmentCount -{ - CEnum Result; - unsigned int SegmentCount; -} COutput_GetSegmentCount; - -/** @private */ -typedef struct COutput_GetSegmentStaticTranslation -{ - CEnum Result; - double Translation[3]; -} COutput_GetSegmentStaticTranslation; - -/** @private */ -typedef struct COutput_GetSegmentStaticRotationHelical -{ - CEnum Result; - double Rotation[3]; -} COutput_GetSegmentStaticRotationHelical; - -/** @private */ -typedef struct COutput_GetSegmentStaticRotationMatrix -{ - CEnum Result; - double Rotation[9]; -} COutput_GetSegmentStaticRotationMatrix; - -/** @private */ -typedef struct COutput_GetSegmentStaticRotationQuaternion -{ - CEnum Result; - double Rotation[4]; -} COutput_GetSegmentStaticRotationQuaternion; - -/** @private */ -typedef struct COutput_GetSegmentStaticRotationEulerXYZ -{ - CEnum Result; - double Rotation[3]; -} COutput_GetSegmentStaticRotationEulerXYZ; - -/** @private */ -typedef struct COutput_GetSegmentGlobalTranslation -{ - CEnum Result; - double Translation[3]; - CBool Occluded; -} COutput_GetSegmentGlobalTranslation; - -/** @private */ -typedef struct COutput_GetSegmentGlobalRotationHelical -{ - CEnum Result; - double Rotation[3]; - CBool Occluded; -} COutput_GetSegmentGlobalRotationHelical; - -/** @private */ -typedef struct COutput_GetSegmentGlobalRotationMatrix -{ - CEnum Result; - double Rotation[9]; - CBool Occluded; -} COutput_GetSegmentGlobalRotationMatrix; - -/** @private */ -typedef struct COutput_GetSegmentGlobalRotationQuaternion -{ - CEnum Result; - double Rotation[4]; - CBool Occluded; -} COutput_GetSegmentGlobalRotationQuaternion; - -/** @private */ -typedef struct COutput_GetSegmentGlobalRotationEulerXYZ -{ - CEnum Result; - double Rotation[3]; - CBool Occluded; -} COutput_GetSegmentGlobalRotationEulerXYZ; - -/** @private */ -typedef struct COutput_GetSegmentLocalTranslation -{ - CEnum Result; - double Translation[3]; - CBool Occluded; -} COutput_GetSegmentLocalTranslation; - -/** @private */ -typedef struct COutput_GetSegmentLocalRotationHelical -{ - CEnum Result; - double Rotation[3]; - CBool Occluded; -} COutput_GetSegmentLocalRotationHelical; - -/** @private */ -typedef struct COutput_GetSegmentLocalRotationMatrix -{ - CEnum Result; - double Rotation[9]; - CBool Occluded; -} COutput_GetSegmentLocalRotationMatrix; - -/** @private */ -typedef struct COutput_GetSegmentLocalRotationQuaternion -{ - CEnum Result; - double Rotation[4]; - CBool Occluded; -} COutput_GetSegmentLocalRotationQuaternion; - -/** @private */ -typedef struct COutput_GetSegmentLocalRotationEulerXYZ -{ - CEnum Result; - double Rotation[3]; - CBool Occluded; -} COutput_GetSegmentLocalRotationEulerXYZ; - -/** @private */ -typedef struct COutput_GetMarkerCount -{ - CEnum Result; - unsigned int MarkerCount; -} COutput_GetMarkerCount; - -/** @private */ -typedef struct COutput_GetMarkerGlobalTranslation -{ - CEnum Result; - double Translation[3]; - CBool Occluded; -} COutput_GetMarkerGlobalTranslation; - -/** @private */ -typedef struct COutput_GetUnlabeledMarkerCount -{ - CEnum Result; - unsigned int MarkerCount; -} COutput_GetUnlabeledMarkerCount; - -/** @private */ -typedef struct COutput_GetUnlabeledMarkerGlobalTranslation -{ - CEnum Result; - double Translation[3]; -} COutput_GetUnlabeledMarkerGlobalTranslation; - -/** @private */ -typedef struct COutput_GetDeviceCount -{ - CEnum Result; - unsigned int DeviceCount; -} COutput_GetDeviceCount; - -/** @private */ -typedef struct COutput_GetDeviceOutputCount -{ - CEnum Result; - unsigned int DeviceOutputCount; -} COutput_GetDeviceOutputCount; - -/** @private */ -typedef struct COutput_GetDeviceOutputValue -{ - CEnum Result; - double Value; - CBool Occluded; -} COutput_GetDeviceOutputValue; - -/** @private */ -typedef struct COutput_GetDeviceOutputSubsamples -{ - CEnum Result; - unsigned int DeviceOutputSubsamples; - CBool Occluded; -} COutput_GetDeviceOutputSubsamples; - -/** @private */ -typedef struct COutput_GetForcePlateCount -{ - CEnum Result; - unsigned int ForcePlateCount; -} COutput_GetForcePlateCount; - -/** @private */ -typedef struct COutput_GetGlobalForceVector -{ - CEnum Result; - double ForceVector[3]; -} COutput_GetGlobalForceVector; - -/** @private */ -typedef struct COutput_GetGlobalMomentVector -{ - CEnum Result; - double MomentVector[3]; -} COutput_GetGlobalMomentVector; - -/** @private */ -typedef struct COutput_GetGlobalCentreOfPressure -{ - CEnum Result; - double CentreOfPressure[3]; -} COutput_GetGlobalCentreOfPressure; - -/** @private */ -typedef struct COutput_GetForcePlateSubsamples -{ - CEnum Result; - unsigned int ForcePlateSubsamples; -} COutput_GetForcePlateSubsamples; - -/** @private */ -typedef struct COutput_GetEyeTrackerCount -{ - CEnum Result; - unsigned int EyeTrackerCount; -} COutput_GetEyeTrackerCount; - -/** @private */ -typedef struct COutput_GetEyeTrackerGlobalPosition -{ - CEnum Result; - double Position[3]; - CBool Occluded; -} COutput_GetEyeTrackerGlobalPosition; - -/** @private */ -typedef struct COutput_GetEyeTrackerGlobalGazeVector -{ - CEnum Result; - double GazeVector[3]; - CBool Occluded; -} COutput_GetEyeTrackerGlobalGazeVector; - -/** @private */ -typedef struct COutput_GetCentroidCount -{ - CEnum Result; - unsigned int CentroidCount; -} COutput_GetCentroidCount; - -/** @private */ -typedef struct COutput_GetFrameRateValue -{ - CEnum Result; - double Value; -} COutput_GetFrameRateValue; - -/** @private */ -typedef struct COutput_GetLabeledMarkerCount -{ - CEnum Result; - unsigned int MarkerCount; -} COutput_GetLabeledMarkerCount; - -/** @private */ -typedef struct COutput_GetIsVideoCamera -{ - CEnum Result; - CBool IsVideoCamera; -} COutput_GetIsVideoCamera; - -/** @private */ -typedef struct COutput_GetCameraId -{ - CEnum Result; - unsigned int CameraId; -} COutput_GetCameraId; - -/** @private */ -typedef struct COutput_GetHardwareFrameNumber -{ - CEnum Result; - unsigned int HardwareFrameNumber; -} COutput_GetHardwareFrameNumber; - -/** @private */ -typedef struct COutput_GetCameraUserId -{ - CEnum Result; - unsigned int CameraUserId; -} COutput_GetCameraUserId; - -/** @private */ -typedef struct COutput_GetCameraCount -{ - CEnum Result; - unsigned int CameraCount; -} COutput_GetCameraCount; - -/** @private */ -typedef struct COutput_GetCentroidPosition -{ - CEnum Result; - double CentroidPosition[2]; - double Radius; -} COutput_GetCentroidPosition; - -/** @private */ -typedef struct COutput_GetMarkerRayContributionCount -{ - CEnum Result; - unsigned int RayContributionsCount; -} COutput_GetMarkerRayContributionCount; - -/** @private */ -typedef struct COutput_GetObjectQuality -{ - CEnum Result; - double Quality; -} COutput_GetObjectQuality; - -/** @private */ -typedef struct COutput_GetCameraResolution -{ - CEnum Result; - unsigned int ResolutionX; - unsigned int ResolutionY; -} COutput_GetCameraResolution; - -/** @private */ -typedef struct COutput_GetGreyscaleBlob -{ - CEnum Result; - unsigned int * BlobLinePositionsX; - unsigned int BlobLinePositionsXSize; - unsigned int * BlobLinePositionsY; - unsigned int BlobLinePositionsYSize; - unsigned char * BlobLinePixelValues; -} COutput_GetGreyscaleBlob; - -/** @private */ -typedef struct COutput_GetFrameRateCount -{ - CEnum Result; - unsigned int Count; -} COutput_GetFrameRateCount; - -/** @private */ -typedef struct COutput_GetMarkerRayContribution -{ - CEnum Result; - unsigned int CameraID; - unsigned int CentroidIndex; -} COutput_GetMarkerRayContribution; - -/** @private */ -typedef struct COutput_GetServerOrientation -{ - CEnum Result; - CEnum Orientation; -} COutput_GetServerOrientation; - -/** @private */ -typedef struct COutput_GetLabeledMarkerGlobalTranslation -{ - CEnum Result; - double Translation[3]; -} COutput_GetLabeledMarkerGlobalTranslation; - -/** @private */ -typedef struct COutput_GetCentroidWeight -{ - CEnum Result; - double Weight; -} COutput_GetCentroidWeight; - -/** @private */ -typedef struct COutput_GetGreyscaleBlobCount -{ - CEnum Result; - unsigned int BlobCount; -} COutput_GetGreyscaleBlobCount; - -/** @private */ -typedef enum -{ - Up, - Down, - Left, - Right, - Forward, - Backward -} CDirection; - -/** @private */ -typedef enum -{ - UnknownServerOrientation, - YUp, - ZUp, -} CServerOrientation; - -/** @private */ -typedef enum -{ - ClientPull, - ClientPullPreFetch, - ServerPush -} CStreamMode; - -/** @private */ -typedef enum -{ - None, - PAL, - NTSC, - NTSCDrop, - Film, - NTSCFilm, - ATSC -} CTimecodeStandard; - -/** @private */ -typedef enum -{ - UnknownDeviceType, - ForcePlate, - EyeTracker -} CDeviceType; - -/** @private */ -typedef enum -{ - UnknownUnit, - Volt, - Newton, - NewtonMeter, - Meter, - Kilogram, - Second, - Ampere, - Kelvin, - Mole, - Candela, - Radian, - Steradian, - MeterSquared, - MeterCubed, - MeterPerSecond, - MeterPerSecondSquared, - RadianPerSecond, - RadianPerSecondSquared, - Hertz, - Joule, - Watt, - Pascal, - Lumen, - Lux, - Coulomb, - Ohm, - Farad, - Weber, - Tesla, - Henry, - Siemens, - Becquerel, - Gray, - Sievert, - Katal -} CUnit; - -/** - * The Result code indicates the success or failure of a function. - */ -typedef enum -{ - UnknownResult, /**< The result is unknown. Treat it as a failure. */ - NotImplemented, /**< The function called has not been implemented in this version of the SDK.*/ - Success, /**< The function call succeeded.*/ - InvalidHostName, /**< The "HostName" parameter passed to Connect() is invalid.*/ - InvalidMulticastIP, /**< The "MulticastIP" parameter was not in the range "224.0.0.0" - "239.255.255.255"*/ - ClientAlreadyConnected, /**< Connect() was called whilst already connected to a DataStream.*/ - ClientConnectionFailed, /**< Connect() could not establish a connection to the DataStream server.*/ - ServerAlreadyTransmittingMulticast, /**< StartTransmittingMulticast() was called when the current DataStream server was already transmitting multicast on behalf of this client.*/ - ServerNotTransmittingMulticast, /**< StopTransmittingMulticast() was called when the current DataStream server was not transmitting multicast on behalf of this client.*/ - NotConnected, /**< You have called a function which requires a connection to the DataStream server, but do not have a connection.*/ - NoFrame, /**< You have called a function which requires a frame to be fetched from the DataStream server, but do not have a frame.*/ - InvalidIndex, /**< An index you have passed to a function is out of range.*/ - InvalidCameraName, /**< The Camera Name you passed to a function is invalid in this frame.*/ - InvalidSubjectName, /**< The Subject Name you passed to a function is invalid in this frame.*/ - InvalidSegmentName, /**< The Segment Name you passed to a function is invalid in this frame.*/ - InvalidMarkerName, /**< The Marker Name you passed to a function is invalid in this frame.*/ - InvalidDeviceName, /**< The Device Name you passed to a function is invalid in this frame.*/ - InvalidDeviceOutputName, /**< The Device Output Name you passed to a function is invalid in this frame.*/ - InvalidLatencySampleName, /**< The Latency Sample Name you passed to a function is invalid in this frame.*/ - CoLinearAxes, /**< The directions passed to SetAxisMapping() contain input which would cause two or more axes to lie along the same line, e.g. "Up" and "Down" are on the same line.*/ - LeftHandedAxes, /**< The directions passed to SetAxisMapping() would result in a left-handed coordinate system. This is not supported in the SDK.*/ - HapticAlreadySet, /**< Haptic feedback is already set.*/ - EarlyDataRequested, /**< Re-timed data requested is from before the first time sample we still have. */ - LateDataRequested /**< Re-timed data requested is too far into the future to be predicted. */ -} CResult; - -#pragma pack(pop) diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/VersionInfoResource.rc2 b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/VersionInfoResource.rc2 deleted file mode 100644 index 00b63df64..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/VersionInfoResource.rc2 +++ /dev/null @@ -1,41 +0,0 @@ -#include "ViconDataStreamSDK_CVersion.h" - -///////////////////////////////////////////////////////////////////////////// -// -// Version -// - -VS_VERSION_INFO VERSIONINFO - FILEVERSION VICONDATASTREAMSDK_C_VERSION_MAJOR,VICONDATASTREAMSDK_C_VERSION_MINOR,VICONDATASTREAMSDK_C_VERSION_POINT,VICONDATASTREAMSDK_C_VERSION_REVISION - PRODUCTVERSION VICONDATASTREAMSDK_C_VERSION_MAJOR,VICONDATASTREAMSDK_C_VERSION_MINOR,VICONDATASTREAMSDK_C_VERSION_POINT,VICONDATASTREAMSDK_C_VERSION_REVISION - FILEFLAGSMASK 0x3fL -#ifdef _DEBUG - FILEFLAGS 0x1L -#else - FILEFLAGS 0x0L -#endif - FILEOS 0x4L - FILETYPE 0x1L - FILESUBTYPE 0x0L -BEGIN - BLOCK "StringFileInfo" - BEGIN - BLOCK "040904b0" - BEGIN - VALUE "CompanyName", VICONDATASTREAMSDK_C_COMPANY_L - VALUE "FileDescription", "Vicon DataStream SDK for C" - VALUE "FileVersion", VICONDATASTREAMSDK_C_FULL_VERSION_STRING_L - VALUE "InternalName", "Vicon DataStream SDK for C" - VALUE "LegalCopyright", VICONDATASTREAMSDK_C_COPYRIGHT_L - VALUE "LegalTrademarks", VICONDATASTREAMSDK_C_TRADEMARK_L - VALUE "OriginalFilename", "ViconDataStreamSDK_C.dll" - VALUE "ProductName", "Vicon DataStream SDK" - VALUE "ProductVersion", VICONDATASTREAMSDK_C_FULL_VERSION_STRING_L - END - END - BLOCK "VarFileInfo" - BEGIN - VALUE "Translation", 0x409, 1200 - END -END - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/ViconDataStreamSDK_C.rc b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/ViconDataStreamSDK_C.rc deleted file mode 100644 index 7a4bb5726..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/ViconDataStreamSDK_C.rc +++ /dev/null @@ -1,61 +0,0 @@ -// Microsoft Visual C++ generated resource script. -// -#include "resource.h" - -#define APSTUDIO_READONLY_SYMBOLS -///////////////////////////////////////////////////////////////////////////// -// -// Generated from the TEXTINCLUDE 2 resource. -// -#include "windows.h" -///////////////////////////////////////////////////////////////////////////// -#undef APSTUDIO_READONLY_SYMBOLS - -///////////////////////////////////////////////////////////////////////////// -// English (U.S.) resources - -#if !defined(AFX_RESOURCE_DLL) || defined(AFX_TARG_ENU) -#ifdef _WIN32 -LANGUAGE LANG_ENGLISH, SUBLANG_ENGLISH_US -#pragma code_page(1252) -#endif //_WIN32 - -#ifdef APSTUDIO_INVOKED -///////////////////////////////////////////////////////////////////////////// -// -// TEXTINCLUDE -// - -1 TEXTINCLUDE -BEGIN - "resource.h\0" -END - -2 TEXTINCLUDE -BEGIN - "#include ""windows.h""\0" -END - -3 TEXTINCLUDE -BEGIN - "#include ""VersionInfoResource.rc2""\r\n" - "\0" -END - -#endif // APSTUDIO_INVOKED - -#endif // English (U.S.) resources -///////////////////////////////////////////////////////////////////////////// - - - -#ifndef APSTUDIO_INVOKED -///////////////////////////////////////////////////////////////////////////// -// -// Generated from the TEXTINCLUDE 3 resource. -// -#include "VersionInfoResource.rc2" - -///////////////////////////////////////////////////////////////////////////// -#endif // not APSTUDIO_INVOKED - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/ViconDataStreamSDK_CVersion.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/ViconDataStreamSDK_CVersion.h deleted file mode 100644 index 55dfd9faa..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/ViconDataStreamSDK_CVersion.h +++ /dev/null @@ -1,27 +0,0 @@ -#define VICONDATASTREAMSDK_C_VERSION_MAJOR 1 -#define VICONDATASTREAMSDK_C_VERSION_MINOR 8 -#define VICONDATASTREAMSDK_C_VERSION_POINT 0 -#define VICONDATASTREAMSDK_C_VERSION_REVISION_CONTROL_SYSTEM "unknown" -#define VICONDATASTREAMSDK_C_VERSION_REVISION 0 -#define VICONDATASTREAMSDK_C_VERSION_MONOTONIC_REVISION 0 -#define VICONDATASTREAMSDK_C_VERSION_PUBLIC_REVISION_STRING "0" -#define VICONDATASTREAMSDK_C_VERSION_RELEASE "" -#define VICONDATASTREAMSDK_C_VERSION_BRANCH "unknown" -#define VICONDATASTREAMSDK_C_VERSION_REPOSITORY "unknown" -#define VICONDATASTREAMSDK_C_VERSION_CHANGESET "0" -#define VICONDATASTREAMSDK_C_VERSION_LOCAL_CHANGESET "0" -#define VICONDATASTREAMSDK_C_VERSION_LOCAL_BRANCH "unknown" - -#define VICONDATASTREAMSDK_C_FULL_VERSION_STRING "1.8.0.0" -#define VICONDATASTREAMSDK_C_VERSION_STRING "1.8" -#define VICONDATASTREAMSDK_C_COMPANY "Vicon Motion Systems Ltd" -#define VICONDATASTREAMSDK_C_COPYRIGHT "Copyright \x00A9 2018 Vicon Motion Systems Ltd. All rights reserved." -#define VICONDATASTREAMSDK_C_TRADEMARK "Vicon\x00AE is a registered trademark of OMG Plc." -#define VICONDATASTREAMSDK_C_PROJECT_NAME "ViconDataStreamSDK_C" - -#define VICONDATASTREAMSDK_C_FULL_VERSION_STRING_L L"1.8.0.0" -#define VICONDATASTREAMSDK_C_VERSION_STRING_L L"1.8" -#define VICONDATASTREAMSDK_C_COMPANY_L L"Vicon Motion Systems Ltd" -#define VICONDATASTREAMSDK_C_COPYRIGHT_L L"Copyright \x00A9 2018 Vicon Motion Systems Ltd. All rights reserved." -#define VICONDATASTREAMSDK_C_TRADEMARK_L L"Vicon\x00AE is a registered trademark of OMG Plc." -#define VICONDATASTREAMSDK_C_PROJECT_NAME_L L"ViconDataStreamSDK_C" diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/makefile b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/makefile deleted file mode 100644 index 5ddcbf873..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/makefile +++ /dev/null @@ -1,117 +0,0 @@ -# Makefile for ViconDataStreamSDK_C - -ifndef VERBOSE -.SILENT : -endif -.SUFFIXES : - -ifdef CONFIG -ifneq ($(CONFIG),Debug) -ifneq ($(CONFIG),InternalRelease) -ifneq ($(CONFIG),Release) -Error: unknown configuration. -endif -endif -endif -else -CONFIG=Debug -endif - -INCLUDEPATHS=../../../../../deployment\..\vicon\Vicon\CrossMarket\DataStream\ViconDataStreamSDK_CPP . ../.. ../../../../thirdparty/Boost/boost-1.58.0-dynamic-linux-x64/installed/include .. -INCLUDEPATHS_Debug=Debug -INCLUDEPATHS_InternalRelease=InternalRelease -INCLUDEPATHS_Release=Release -LIBRARYPATHS=../../../../thirdparty/Boost/boost-1.58.0-dynamic-linux-x64/installed/lib -LIBRARYPATHS_Debug=../../../../lib/Debug ../../../../bin/Debug -LIBRARYPATHS_InternalRelease=../../../../lib/InternalRelease ../../../../bin/InternalRelease -LIBRARYPATHS_Release=../../../../lib/Release ../../../../bin/Release -DEPENDENCIES=ViconDataStreamSDKCore ViconDataStreamSDK_CPP ViconCGStreamClientSDK ViconCGStreamClient ViconCGStream StreamCommon -LIBRARIES= -LIBRARIES_Debug=boost_wserialization-mt-d boost_wave-mt-d boost_unit_test_framework-mt-d boost_timer-mt-d boost_thread-mt-d boost_system-mt-d boost_signals-mt-d boost_serialization-mt-d boost_regex-mt-d boost_random-mt-d boost_python-mt-d boost_program_options-mt-d boost_prg_exec_monitor-mt-d boost_math_tr1l-mt-d boost_math_tr1f-mt-d boost_math_tr1-mt-d boost_math_c99l-mt-d boost_math_c99f-mt-d boost_math_c99-mt-d boost_log_setup-mt-d boost_log-mt-d boost_locale-mt-d boost_iostreams-mt-d boost_graph-mt-d boost_filesystem-mt-d boost_date_time-mt-d boost_coroutine-mt-d boost_context-mt-d boost_container-mt-d boost_chrono-mt-d boost_atomic-mt-d -LIBRARIES_InternalRelease=boost_wserialization-mt boost_wave-mt boost_unit_test_framework-mt boost_timer-mt boost_thread-mt boost_system-mt boost_signals-mt boost_serialization-mt boost_regex-mt boost_random-mt boost_python-mt boost_program_options-mt boost_prg_exec_monitor-mt boost_math_tr1l-mt boost_math_tr1f-mt boost_math_tr1-mt boost_math_c99l-mt boost_math_c99f-mt boost_math_c99-mt boost_log_setup-mt boost_log-mt boost_locale-mt boost_iostreams-mt boost_graph-mt boost_filesystem-mt boost_date_time-mt boost_coroutine-mt boost_context-mt boost_container-mt boost_chrono-mt boost_atomic-mt -LIBRARIES_Release=boost_wserialization-mt boost_wave-mt boost_unit_test_framework-mt boost_timer-mt boost_thread-mt boost_system-mt boost_signals-mt boost_serialization-mt boost_regex-mt boost_random-mt boost_python-mt boost_program_options-mt boost_prg_exec_monitor-mt boost_math_tr1l-mt boost_math_tr1f-mt boost_math_tr1-mt boost_math_c99l-mt boost_math_c99f-mt boost_math_c99-mt boost_log_setup-mt boost_log-mt boost_locale-mt boost_iostreams-mt boost_graph-mt boost_filesystem-mt boost_date_time-mt boost_coroutine-mt boost_context-mt boost_container-mt boost_chrono-mt boost_atomic-mt -DEFINES=TCM_LINUX TCM_UNIX _EXPORTING PROJECT_SOURCE_PATH=\"/home/swtest/workspace/DatastreamSDK_Linux_64_Branch_Release/Binary/bin/Release/deployment/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C\" -DEFINES_Debug=_DEBUG -DEFINES_InternalRelease=VICON_INTERNAL_RELEASE NDEBUG -DEFINES_Release=NDEBUG TCM_OFF_SITE -ENV_CPU=x64 - -SOURCEDIRECTORY=../../../.. -PROJECTPATH=. -BINARYDIRECTORY=../../../.. -INTERMEDIATEDIRECTORY=. -LIBRARYDIRECTORY=../../../../lib -OUTPUTDIRECTORY=../../../../bin - -include $(BINARYDIRECTORY)/gcc.mk - -HIDE_BOOST_SCRIPT=hide_boost_version_script -ifneq ($(HIDE_BOOST),) - HIDE_BOOST_LD_PARAM= -Wl,--version-script=$(HIDE_BOOST_SCRIPT) - HIDE_BOOST_LD_PREREQ=$(HIDE_BOOST_SCRIPT) -endif -all: all_$(CONFIG) - -all_Debug: $(OUTPUTDIRECTORY)/$(CONFIG)/libViconDataStreamSDK_C.so -all_InternalRelease: $(OUTPUTDIRECTORY)/$(CONFIG)/libViconDataStreamSDK_C.so -all_Release: $(OUTPUTDIRECTORY)/$(CONFIG)/libViconDataStreamSDK_C.so - -OBJECTS=$(CONFIG)/CClient.o $(CONFIG)/CRetimingClient.o - -CXXFLAGS=$(CXXFLAGS_$(CONFIG)) $(L_INCLUDEPATHS) $(L_INCLUDEPATHS_$(CONFIG)) $(L_INCLUDES) $(L_INCLUDES_$(CONFIG)) $(L_DEFINES) $(L_DEFINES_$(CONFIG)) -CCFLAGS=$(CCFLAGS_$(CONFIG)) $(L_INCLUDEPATHS) $(L_INCLUDEPATHS_$(CONFIG)) $(L_INCLUDES) $(L_INCLUDES_$(CONFIG)) $(L_DEFINES) $(L_DEFINES_$(CONFIG)) -LDFLAGS=$(LDFLAGS_$(CONFIG)) $(L_LIBRARYPATHS) $(L_LIBRARYPATHS_$(CONFIG)) -CXXFLAGS+=-Wno-unused-local-typedefs -CXXFLAGS+=-Wno-delete-non-virtual-dtor -std=c++14 -#Android toolchain does not include librt but integrates some of its functionality into Android libc. -ifndef ANDROID_TARGET_ARCH -LDFLAGS+=-lrt -endif - - -$(OUTPUTDIRECTORY)/Debug/libViconDataStreamSDK_C.so: makefile $(OBJECTS) $(LIBRARYDIRECTORY)/$(CONFIG)/libStreamCommon.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStream.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClient.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClientSDK.a $(OUTPUTDIRECTORY)/$(CONFIG)/libViconDataStreamSDK_CPP.so $(LIBRARYDIRECTORY)/$(CONFIG)/libViconDataStreamSDKCore.a $(HIDE_BOOST_LD_PREREQ) - @echo \[1\;32mLinking dll $@\ - @mkdir -p $(@D) - $(LD) -fPIC -shared -Wl,--as-needed $(LDFLAGS) -o $@ $(OBJECTS) -Wl,--start-group $(L_DEPENDENCIES) $(L_LIBRARIES) $(L_LIBRARIES_$(CONFIG)) -Wl,--end-group -pthread -ldl -Wl,-rpath='$$ORIGIN':$(@D) $(HIDE_BOOST_LD_PARAM) - -$(OUTPUTDIRECTORY)/InternalRelease/libViconDataStreamSDK_C.so: makefile $(OBJECTS) $(LIBRARYDIRECTORY)/$(CONFIG)/libStreamCommon.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStream.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClient.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClientSDK.a $(OUTPUTDIRECTORY)/$(CONFIG)/libViconDataStreamSDK_CPP.so $(LIBRARYDIRECTORY)/$(CONFIG)/libViconDataStreamSDKCore.a $(HIDE_BOOST_LD_PREREQ) - @echo \[1\;32mLinking dll $@\ - @mkdir -p $(@D) - $(LD) -fPIC -shared -Wl,--as-needed $(LDFLAGS) -o $@ $(OBJECTS) -Wl,--start-group $(L_DEPENDENCIES) $(L_LIBRARIES) $(L_LIBRARIES_$(CONFIG)) -Wl,--end-group -pthread -ldl -Wl,-rpath='$$ORIGIN':$(@D) $(HIDE_BOOST_LD_PARAM) - -$(OUTPUTDIRECTORY)/Release/libViconDataStreamSDK_C.so: makefile $(OBJECTS) $(LIBRARYDIRECTORY)/$(CONFIG)/libStreamCommon.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStream.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClient.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClientSDK.a $(OUTPUTDIRECTORY)/$(CONFIG)/libViconDataStreamSDK_CPP.so $(LIBRARYDIRECTORY)/$(CONFIG)/libViconDataStreamSDKCore.a $(HIDE_BOOST_LD_PREREQ) - @echo \[1\;32mLinking dll $@\ - @mkdir -p $(@D) - $(LD) -fPIC -shared -Wl,--as-needed $(LDFLAGS) -o $@ $(OBJECTS) -Wl,--start-group $(L_DEPENDENCIES) $(L_LIBRARIES) $(L_LIBRARIES_$(CONFIG)) -Wl,--end-group -pthread -ldl -Wl,-rpath='$$ORIGIN':$(@D) $(HIDE_BOOST_LD_PARAM) - -# Source Files -$(CONFIG)/CClient.o: makefile $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/CClient.cpp - @echo \[1\;34mCompiling CClient.cpp\ - @mkdir -p $(@D) - find $(CONFIG) -name *.gch -exec cp '{}' . \; - $(CXX) -fPIC -MMD -MP -I$(CONFIG)/ $(CXXFLAGS) -o $@ -c $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/CClient.cpp - --include $(CONFIG)/CClient.d - -$(CONFIG)/CRetimingClient.o: makefile $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/CRetimingClient.cpp - @echo \[1\;34mCompiling CRetimingClient.cpp\ - @mkdir -p $(@D) - find $(CONFIG) -name *.gch -exec cp '{}' . \; - $(CXX) -fPIC -MMD -MP -I$(CONFIG)/ $(CXXFLAGS) -o $@ -c $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/CRetimingClient.cpp - --include $(CONFIG)/CRetimingClient.d - -# Header Files -# Resource Files -# Other Files - -clean: - @echo \[1\;31mCleaning $(CONFIG) build\ - find . -path '*/$(CONFIG)/*' \( -name '*.[od]' -o -name '*.gch' \) -exec rm -f {} ';' - rm -f moc_*.cxx - -$(HIDE_BOOST_SCRIPT): makefile - echo -n >$@ - echo "{" >>$@ - echo " local: *N5boost*; *NK5boost*;" >>$@ - echo "};" >>$@ diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/resource.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_C/resource.h deleted file mode 100644 index e69de29bb..000000000 diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/CoreAdapters.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/CoreAdapters.h deleted file mode 100644 index a7aaec85a..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/CoreAdapters.h +++ /dev/null @@ -1,202 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -#include "IDataStreamClientBase.h" -#include - -// This function is provided to insulate us from changes to ViconDataStreamSDK::Core::Axis::Enum -static ViconDataStreamSDK::Core::Direction::Enum Adapt(const ViconDataStreamSDK::CPP::Direction::Enum i_Direction) -{ - switch (i_Direction) - { - default: - case ViconDataStreamSDK::CPP::Direction::Up: return ViconDataStreamSDK::Core::Direction::Up; - case ViconDataStreamSDK::CPP::Direction::Down: return ViconDataStreamSDK::Core::Direction::Down; - case ViconDataStreamSDK::CPP::Direction::Left: return ViconDataStreamSDK::Core::Direction::Left; - case ViconDataStreamSDK::CPP::Direction::Right: return ViconDataStreamSDK::Core::Direction::Right; - case ViconDataStreamSDK::CPP::Direction::Forward: return ViconDataStreamSDK::Core::Direction::Forward; - case ViconDataStreamSDK::CPP::Direction::Backward: return ViconDataStreamSDK::Core::Direction::Backward; - } -} - -// This function is provided to insulate us from changes to ViconDataStreamSDK::Core::Axis::Enum -static ViconDataStreamSDK::CPP::Direction::Enum Adapt(const ViconDataStreamSDK::Core::Direction::Enum i_Direction) -{ - switch (i_Direction) - { - default: - case ViconDataStreamSDK::Core::Direction::Up: return ViconDataStreamSDK::CPP::Direction::Up; - case ViconDataStreamSDK::Core::Direction::Down: return ViconDataStreamSDK::CPP::Direction::Down; - case ViconDataStreamSDK::Core::Direction::Left: return ViconDataStreamSDK::CPP::Direction::Left; - case ViconDataStreamSDK::Core::Direction::Right: return ViconDataStreamSDK::CPP::Direction::Right; - case ViconDataStreamSDK::Core::Direction::Forward: return ViconDataStreamSDK::CPP::Direction::Forward; - case ViconDataStreamSDK::Core::Direction::Backward: return ViconDataStreamSDK::CPP::Direction::Backward; - } -} - -static ViconDataStreamSDK::CPP::ServerOrientation::Enum Adapt(const ViconDataStreamSDK::Core::ServerOrientation::Enum i_Orientation) -{ - switch (i_Orientation) - { - default: - case ViconDataStreamSDK::Core::ServerOrientation::Unknown: return ViconDataStreamSDK::CPP::ServerOrientation::Unknown; - case ViconDataStreamSDK::Core::ServerOrientation::YUp: return ViconDataStreamSDK::CPP::ServerOrientation::YUp; - case ViconDataStreamSDK::Core::ServerOrientation::ZUp: return ViconDataStreamSDK::CPP::ServerOrientation::ZUp; - } -} - -static ViconDataStreamSDK::Core::ServerOrientation::Enum Adapt(const ViconDataStreamSDK::CPP::ServerOrientation::Enum i_Orientation) -{ - switch (i_Orientation) - { - default: - case ViconDataStreamSDK::CPP::ServerOrientation::Unknown: return ViconDataStreamSDK::Core::ServerOrientation::Unknown; - case ViconDataStreamSDK::CPP::ServerOrientation::YUp: return ViconDataStreamSDK::Core::ServerOrientation::YUp; - case ViconDataStreamSDK::CPP::ServerOrientation::ZUp: return ViconDataStreamSDK::Core::ServerOrientation::ZUp; - } -} - -// This function is provided to insulate us from changes to ViconDataStreamSDK::CPP::StreamMode::Enum -static ViconDataStreamSDK::Core::StreamMode::Enum Adapt(ViconDataStreamSDK::CPP::StreamMode::Enum i_Mode) -{ - switch (i_Mode) - { - default: - case ViconDataStreamSDK::CPP::StreamMode::ClientPull: return ViconDataStreamSDK::Core::StreamMode::ClientPull; - case ViconDataStreamSDK::CPP::StreamMode::ClientPullPreFetch: return ViconDataStreamSDK::Core::StreamMode::ClientPullPreFetch; - case ViconDataStreamSDK::CPP::StreamMode::ServerPush: return ViconDataStreamSDK::Core::StreamMode::ServerPush; - } -} -// This function is provided to insulate us from changes to ViconDataStreamSDK::Core::Result::Enum -static ViconDataStreamSDK::CPP::Result::Enum Adapt(ViconDataStreamSDK::Core::Result::Enum i_Result) -{ - switch (i_Result) - { - case ViconDataStreamSDK::Core::Result::Unknown: return ViconDataStreamSDK::CPP::Result::Unknown; - case ViconDataStreamSDK::Core::Result::NotImplemented: return ViconDataStreamSDK::CPP::Result::NotImplemented; - case ViconDataStreamSDK::Core::Result::Success: return ViconDataStreamSDK::CPP::Result::Success; - case ViconDataStreamSDK::Core::Result::InvalidHostName: return ViconDataStreamSDK::CPP::Result::InvalidHostName; - case ViconDataStreamSDK::Core::Result::InvalidMulticastIP: return ViconDataStreamSDK::CPP::Result::InvalidMulticastIP; - case ViconDataStreamSDK::Core::Result::ClientAlreadyConnected: return ViconDataStreamSDK::CPP::Result::ClientAlreadyConnected; - case ViconDataStreamSDK::Core::Result::ClientConnectionFailed: return ViconDataStreamSDK::CPP::Result::ClientConnectionFailed; - case ViconDataStreamSDK::Core::Result::ServerAlreadyTransmittingMulticast: return ViconDataStreamSDK::CPP::Result::ServerAlreadyTransmittingMulticast; - case ViconDataStreamSDK::Core::Result::ServerNotTransmittingMulticast: return ViconDataStreamSDK::CPP::Result::ServerNotTransmittingMulticast; - case ViconDataStreamSDK::Core::Result::NotConnected: return ViconDataStreamSDK::CPP::Result::NotConnected; - case ViconDataStreamSDK::Core::Result::NoFrame: return ViconDataStreamSDK::CPP::Result::NoFrame; - case ViconDataStreamSDK::Core::Result::InvalidIndex: return ViconDataStreamSDK::CPP::Result::InvalidIndex; - case ViconDataStreamSDK::Core::Result::InvalidCameraName: return ViconDataStreamSDK::CPP::Result::InvalidCameraName; - case ViconDataStreamSDK::Core::Result::InvalidSubjectName: return ViconDataStreamSDK::CPP::Result::InvalidSubjectName; - case ViconDataStreamSDK::Core::Result::InvalidSegmentName: return ViconDataStreamSDK::CPP::Result::InvalidSegmentName; - case ViconDataStreamSDK::Core::Result::InvalidMarkerName: return ViconDataStreamSDK::CPP::Result::InvalidMarkerName; - case ViconDataStreamSDK::Core::Result::InvalidDeviceName: return ViconDataStreamSDK::CPP::Result::InvalidDeviceName; - case ViconDataStreamSDK::Core::Result::InvalidDeviceOutputName: return ViconDataStreamSDK::CPP::Result::InvalidDeviceOutputName; - case ViconDataStreamSDK::Core::Result::InvalidLatencySampleName: return ViconDataStreamSDK::CPP::Result::InvalidLatencySampleName; - case ViconDataStreamSDK::Core::Result::CoLinearAxes: return ViconDataStreamSDK::CPP::Result::CoLinearAxes; - case ViconDataStreamSDK::Core::Result::LeftHandedAxes: return ViconDataStreamSDK::CPP::Result::LeftHandedAxes; - case ViconDataStreamSDK::Core::Result::HapticAlreadySet: return ViconDataStreamSDK::CPP::Result::HapticAlreadySet; - case ViconDataStreamSDK::Core::Result::EarlyDataRequested: return ViconDataStreamSDK::CPP::Result::EarlyDataRequested; - case ViconDataStreamSDK::Core::Result::LateDataRequested: return ViconDataStreamSDK::CPP::Result::LateDataRequested; - case ViconDataStreamSDK::Core::Result::InvalidOperation: return ViconDataStreamSDK::CPP::Result::InvalidOperation; - case ViconDataStreamSDK::Core::Result::NullClient: - default: assert(!"Unexpected result code returned"); - return ViconDataStreamSDK::CPP::Result::Unknown; - } -} - -// This function is provided to insulate us from changes to ViconDataStreamSDK::Core::TimecodeStandard::Enum -static ViconDataStreamSDK::CPP::TimecodeStandard::Enum Adapt(ViconDataStreamSDK::Core::TimecodeStandard::Enum i_TimecodeStandard) -{ - switch (i_TimecodeStandard) - { - case ViconDataStreamSDK::Core::TimecodeStandard::PAL: return ViconDataStreamSDK::CPP::TimecodeStandard::PAL; - case ViconDataStreamSDK::Core::TimecodeStandard::NTSC: return ViconDataStreamSDK::CPP::TimecodeStandard::NTSC; - case ViconDataStreamSDK::Core::TimecodeStandard::NTSCDrop: return ViconDataStreamSDK::CPP::TimecodeStandard::NTSCDrop; - case ViconDataStreamSDK::Core::TimecodeStandard::Film: return ViconDataStreamSDK::CPP::TimecodeStandard::Film; - case ViconDataStreamSDK::Core::TimecodeStandard::NTSCFilm: return ViconDataStreamSDK::CPP::TimecodeStandard::NTSCFilm; - case ViconDataStreamSDK::Core::TimecodeStandard::ATSC: return ViconDataStreamSDK::CPP::TimecodeStandard::ATSC; - default: assert(!"Unexpected result code returned"); - case ViconDataStreamSDK::Core::TimecodeStandard::None: return ViconDataStreamSDK::CPP::TimecodeStandard::None; - } -} - -// This function is provided to insulate us from changes to ViconDataStreamSDK::Core::DeviceType::Enum -static ViconDataStreamSDK::CPP::DeviceType::Enum Adapt(ViconDataStreamSDK::Core::DeviceType::Enum i_DeviceType) -{ - switch (i_DeviceType) - { - case ViconDataStreamSDK::Core::DeviceType::ForcePlate: return ViconDataStreamSDK::CPP::DeviceType::ForcePlate; - case ViconDataStreamSDK::Core::DeviceType::EyeTracker: return ViconDataStreamSDK::CPP::DeviceType::EyeTracker; - default: assert(!"Unexpected result code returned"); - case ViconDataStreamSDK::Core::DeviceType::Unknown: return ViconDataStreamSDK::CPP::DeviceType::Unknown; - } -} - -// This function is provided to insulate us from changes to ViconDataStreamSDK::Core::Unit::Enum -static ViconDataStreamSDK::CPP::Unit::Enum Adapt(ViconDataStreamSDK::Core::Unit::Enum i_Unit) -{ - switch (i_Unit) - { - case ViconDataStreamSDK::Core::Unit::Volt: return ViconDataStreamSDK::CPP::Unit::Volt; - case ViconDataStreamSDK::Core::Unit::Newton: return ViconDataStreamSDK::CPP::Unit::Newton; - case ViconDataStreamSDK::Core::Unit::NewtonMeter: return ViconDataStreamSDK::CPP::Unit::NewtonMeter; - case ViconDataStreamSDK::Core::Unit::Meter: return ViconDataStreamSDK::CPP::Unit::Meter; - case ViconDataStreamSDK::Core::Unit::Kilogram: return ViconDataStreamSDK::CPP::Unit::Kilogram; - case ViconDataStreamSDK::Core::Unit::Second: return ViconDataStreamSDK::CPP::Unit::Second; - case ViconDataStreamSDK::Core::Unit::Ampere: return ViconDataStreamSDK::CPP::Unit::Ampere; - case ViconDataStreamSDK::Core::Unit::Kelvin: return ViconDataStreamSDK::CPP::Unit::Kelvin; - case ViconDataStreamSDK::Core::Unit::Mole: return ViconDataStreamSDK::CPP::Unit::Mole; - case ViconDataStreamSDK::Core::Unit::Candela: return ViconDataStreamSDK::CPP::Unit::Candela; - case ViconDataStreamSDK::Core::Unit::Radian: return ViconDataStreamSDK::CPP::Unit::Radian; - case ViconDataStreamSDK::Core::Unit::Steradian: return ViconDataStreamSDK::CPP::Unit::Steradian; - case ViconDataStreamSDK::Core::Unit::MeterSquared: return ViconDataStreamSDK::CPP::Unit::MeterSquared; - case ViconDataStreamSDK::Core::Unit::MeterCubed: return ViconDataStreamSDK::CPP::Unit::MeterCubed; - case ViconDataStreamSDK::Core::Unit::MeterPerSecond: return ViconDataStreamSDK::CPP::Unit::MeterPerSecond; - case ViconDataStreamSDK::Core::Unit::MeterPerSecondSquared: return ViconDataStreamSDK::CPP::Unit::MeterPerSecondSquared; - case ViconDataStreamSDK::Core::Unit::RadianPerSecond: return ViconDataStreamSDK::CPP::Unit::RadianPerSecond; - case ViconDataStreamSDK::Core::Unit::RadianPerSecondSquared: return ViconDataStreamSDK::CPP::Unit::RadianPerSecondSquared; - case ViconDataStreamSDK::Core::Unit::Hertz: return ViconDataStreamSDK::CPP::Unit::Hertz; - case ViconDataStreamSDK::Core::Unit::Joule: return ViconDataStreamSDK::CPP::Unit::Joule; - case ViconDataStreamSDK::Core::Unit::Watt: return ViconDataStreamSDK::CPP::Unit::Watt; - case ViconDataStreamSDK::Core::Unit::Pascal: return ViconDataStreamSDK::CPP::Unit::Pascal; - case ViconDataStreamSDK::Core::Unit::Lumen: return ViconDataStreamSDK::CPP::Unit::Lumen; - case ViconDataStreamSDK::Core::Unit::Lux: return ViconDataStreamSDK::CPP::Unit::Lux; - case ViconDataStreamSDK::Core::Unit::Coulomb: return ViconDataStreamSDK::CPP::Unit::Coulomb; - case ViconDataStreamSDK::Core::Unit::Ohm: return ViconDataStreamSDK::CPP::Unit::Ohm; - case ViconDataStreamSDK::Core::Unit::Farad: return ViconDataStreamSDK::CPP::Unit::Farad; - case ViconDataStreamSDK::Core::Unit::Weber: return ViconDataStreamSDK::CPP::Unit::Weber; - case ViconDataStreamSDK::Core::Unit::Tesla: return ViconDataStreamSDK::CPP::Unit::Tesla; - case ViconDataStreamSDK::Core::Unit::Henry: return ViconDataStreamSDK::CPP::Unit::Henry; - case ViconDataStreamSDK::Core::Unit::Siemens: return ViconDataStreamSDK::CPP::Unit::Siemens; - case ViconDataStreamSDK::Core::Unit::Becquerel: return ViconDataStreamSDK::CPP::Unit::Becquerel; - case ViconDataStreamSDK::Core::Unit::Gray: return ViconDataStreamSDK::CPP::Unit::Gray; - case ViconDataStreamSDK::Core::Unit::Sievert: return ViconDataStreamSDK::CPP::Unit::Sievert; - case ViconDataStreamSDK::Core::Unit::Katal: return ViconDataStreamSDK::CPP::Unit::Katal; - - default: assert(!"Unexpected result code returned"); - case ViconDataStreamSDK::Core::Unit::Unknown: return ViconDataStreamSDK::CPP::Unit::Unknown; - } -} - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/DataStreamClient.cpp b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/DataStreamClient.cpp deleted file mode 100644 index 023ac666e..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/DataStreamClient.cpp +++ /dev/null @@ -1,1488 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// - -// We are going to export these from a DLL -#define _EXPORTING - -#include "DataStreamClient.h" -#include -#include -#include "StringFactory.h" - -#include "CoreAdapters.h" - -namespace ViconDataStreamSDK -{ -namespace CPP -{ - - - - // This class just wraps something whose identity we don't want to expose - // in our header file - class ClientImpl - { - public: - ClientImpl() - : m_pCoreClient( new ViconDataStreamSDK::Core::VClient() ) - , m_pStringFactory( new VStringFactory() ) - { - } - - std::shared_ptr< ViconDataStreamSDK::Core::VClient > m_pCoreClient; - std::shared_ptr< VStringFactory > m_pStringFactory; - }; - - - // Constructor - CLASS_DECLSPEC - Client::Client() - : m_pClientImpl( new ClientImpl() ) - { - } - - // Destructor - CLASS_DECLSPEC - Client::~Client() - { - delete m_pClientImpl; - m_pClientImpl = 0; - } - - // GetVersion - CLASS_DECLSPEC - Output_GetVersion Client::GetVersion() const - { - Output_GetVersion Output; - m_pClientImpl->m_pCoreClient->GetVersion( Output.Major, - Output.Minor, - Output.Point ); - - return Output; - } - - // Connect - CLASS_DECLSPEC - Output_Connect Client::Connect( const String & HostName ) - { - std::shared_ptr< ViconCGStreamClientSDK::ICGClient > pClient( ViconCGStreamClientSDK::ICGClient::CreateCGClient(), - boost::bind( &ViconCGStreamClientSDK::ICGClient::Destroy, _1 ) ); - Output_Connect Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->Connect( pClient, HostName ) ); - - return Output; - } - - // ConnectToMulticast - CLASS_DECLSPEC - Output_ConnectToMulticast Client::ConnectToMulticast( const String & LocalIP, - const String & MulticastIP ) - { - std::shared_ptr< ViconCGStreamClientSDK::ICGClient > pClient( ViconCGStreamClientSDK::ICGClient::CreateCGClient(), - boost::bind( &ViconCGStreamClientSDK::ICGClient::Destroy, _1 ) ); - Output_ConnectToMulticast Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->ConnectToMulticast( pClient, LocalIP, MulticastIP ) ); - - return Output; - } - - // Disconnect - CLASS_DECLSPEC - Output_Disconnect Client::Disconnect() - { - Output_Disconnect Output; - - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->Disconnect() ); - - return Output; - } - - // IsConnected - CLASS_DECLSPEC - Output_IsConnected Client::IsConnected() const - { - Output_IsConnected Output; - Output.Connected = m_pClientImpl->m_pCoreClient->IsConnected(); - - return Output; - } - - // StartTransmittingMulticast - CLASS_DECLSPEC - Output_StartTransmittingMulticast Client::StartTransmittingMulticast( const String & ServerIP, - const String & MulticastIP ) - { - Output_StartTransmittingMulticast Output; - - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->StartTransmittingMulticast( ServerIP, MulticastIP ) ); - - return Output; - } - - // StopTransmittingMulticast - CLASS_DECLSPEC - Output_StopTransmittingMulticast Client::StopTransmittingMulticast() - { - Output_StopTransmittingMulticast Output; - - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->StopTransmittingMulticast() ); - - return Output; - } - - // SetBufferSize - CLASS_DECLSPEC - void Client::SetBufferSize( unsigned int i_BufferSize ) - { - m_pClientImpl->m_pCoreClient->SetBufferSize( i_BufferSize ); - } - - // EnableSegmentData - CLASS_DECLSPEC - Output_EnableSegmentData Client::EnableSegmentData() - { - Output_EnableSegmentData Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->EnableSegmentData() ); - - return Output; - } - - // EnableMarkerData - CLASS_DECLSPEC - Output_EnableMarkerData Client::EnableMarkerData() - { - Output_EnableMarkerData Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->EnableMarkerData() ); - - return Output; - } - - // EnableUnlabeledMarkerData - CLASS_DECLSPEC - Output_EnableUnlabeledMarkerData Client::EnableUnlabeledMarkerData() - { - Output_EnableUnlabeledMarkerData Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->EnableUnlabeledMarkerData() ); - - return Output; - } - - // EnableMarkerRayData - CLASS_DECLSPEC - ViconDataStreamSDK::CPP::Output_EnableMarkerRayData Client::EnableMarkerRayData() - { - Output_EnableMarkerRayData Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->EnableMarkerRayData() ); - - return Output; - } - - // EnableDeviceData - CLASS_DECLSPEC - Output_EnableDeviceData Client::EnableDeviceData() - { - Output_EnableDeviceData Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->EnableDeviceData() ); - - return Output; - } - - // Enable centroid data - CLASS_DECLSPEC - Output_EnableCentroidData Client::EnableCentroidData() - { - Output_EnableCentroidData Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->EnableCentroidData() ); - return Output; - } - - // Enable greyscale data - CLASS_DECLSPEC - Output_EnableGreyscaleData Client::EnableGreyscaleData() - { - Output_EnableGreyscaleData Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->EnableGreyscaleData() ); - return Output; - } - - // Enable video data - CLASS_DECLSPEC - Output_EnableVideoData Client::EnableVideoData() - { - Output_EnableVideoData Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->EnableVideoData() ); - return Output; - } - - // Enable debug data - CLASS_DECLSPEC - Output_EnableDebugData Client::EnableDebugData() - { - Output_EnableDebugData Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->EnableDebugData() ); - return Output; - } - - // DisableSegmentData - CLASS_DECLSPEC - Output_DisableSegmentData Client::DisableSegmentData() - { - Output_DisableSegmentData Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->DisableSegmentData() ); - - return Output; - } - - // DisableMarkerData - CLASS_DECLSPEC - Output_DisableMarkerData Client::DisableMarkerData() - { - Output_DisableMarkerData Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->DisableMarkerData() ); - - return Output; - } - - // DisableUnlabeledMarkerData - CLASS_DECLSPEC - Output_DisableUnlabeledMarkerData Client::DisableUnlabeledMarkerData() - { - Output_DisableUnlabeledMarkerData Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->DisableUnlabeledMarkerData() ); - - return Output; - } - - // DisableMarkerRayData - CLASS_DECLSPEC - ViconDataStreamSDK::CPP::Output_DisableMarkerRayData Client::DisableMarkerRayData() - { - Output_DisableMarkerRayData Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->DisableMarkerRayData() ); - - return Output; - } - - // DisableDeviceData - CLASS_DECLSPEC - Output_DisableDeviceData Client::DisableDeviceData() - { - Output_DisableDeviceData Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->DisableDeviceData() ); - - return Output; - } - - // Disable centroid data - CLASS_DECLSPEC - Output_DisableCentroidData Client::DisableCentroidData() - { - Output_DisableCentroidData Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->DisableCentroidData() ); - return Output; - } - - // Disable greyscale data - CLASS_DECLSPEC - Output_DisableGreyscaleData Client::DisableGreyscaleData() - { - Output_DisableGreyscaleData Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->DisableGreyscaleData() ); - return Output; - } - - // Disable video data - CLASS_DECLSPEC - Output_DisableVideoData Client::DisableVideoData() - { - Output_DisableVideoData Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->DisableVideoData() ); - return Output; - } - - // Disable debug data - CLASS_DECLSPEC - Output_DisableDebugData Client::DisableDebugData() - { - Output_DisableDebugData Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->DisableDebugData() ); - return Output; - } - - - // IsSegmentDataEnabled - CLASS_DECLSPEC - Output_IsSegmentDataEnabled Client::IsSegmentDataEnabled() const - { - Output_IsSegmentDataEnabled Output; - Output.Enabled = m_pClientImpl->m_pCoreClient->IsSegmentDataEnabled(); - - return Output; - } - - // IsMarkerDataEnabled - CLASS_DECLSPEC - Output_IsMarkerDataEnabled Client::IsMarkerDataEnabled() const - { - Output_IsMarkerDataEnabled Output; - Output.Enabled = m_pClientImpl->m_pCoreClient->IsMarkerDataEnabled(); - - return Output; - } - - // IsUnlabeledMarkerDataEnabled - CLASS_DECLSPEC - Output_IsUnlabeledMarkerDataEnabled Client::IsUnlabeledMarkerDataEnabled() const - { - Output_IsUnlabeledMarkerDataEnabled Output; - Output.Enabled = m_pClientImpl->m_pCoreClient->IsUnlabeledMarkerDataEnabled(); - - return Output; - } - - // IsMarkerRayDataEnabled - CLASS_DECLSPEC - ViconDataStreamSDK::CPP::Output_IsMarkerRayDataEnabled Client::IsMarkerRayDataEnabled() const - { - Output_IsMarkerRayDataEnabled Output; - Output.Enabled = m_pClientImpl->m_pCoreClient->IsMarkerRayDataEnabled(); - - return Output; - } - - // IsDeviceDataEnabled - CLASS_DECLSPEC - Output_IsDeviceDataEnabled Client::IsDeviceDataEnabled() const - { - Output_IsDeviceDataEnabled Output; - Output.Enabled = m_pClientImpl->m_pCoreClient->IsDeviceDataEnabled(); - - return Output; - } - - // IsCentroidDataEnabled - CLASS_DECLSPEC - Output_IsCentroidDataEnabled Client::IsCentroidDataEnabled() const - { - Output_IsCentroidDataEnabled Output; - Output.Enabled = m_pClientImpl->m_pCoreClient->IsCentroidDataEnabled(); - - return Output; - } - - // IsGreyscaleDataEnabled - CLASS_DECLSPEC - Output_IsGreyscaleDataEnabled Client::IsGreyscaleDataEnabled() const - { - Output_IsGreyscaleDataEnabled Output; - Output.Enabled = m_pClientImpl->m_pCoreClient->IsGreyscaleDataEnabled(); - - return Output; - } - - // IsVideoDataEnabled - CLASS_DECLSPEC - Output_IsVideoDataEnabled Client::IsVideoDataEnabled() const - { - Output_IsVideoDataEnabled Output; - Output.Enabled = m_pClientImpl->m_pCoreClient->IsVideoDataEnabled(); - - return Output; - } - - // IsDebugDataEnabled - CLASS_DECLSPEC - Output_IsDebugDataEnabled Client::IsDebugDataEnabled() const - { - Output_IsDebugDataEnabled Output; - Output.Enabled = m_pClientImpl->m_pCoreClient->IsDebugDataEnabled(); - - return Output; - } - - // SetStreamMode - CLASS_DECLSPEC - Output_SetStreamMode Client::SetStreamMode( const StreamMode::Enum Mode ) - { - Output_SetStreamMode Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->SetStreamMode( Adapt( Mode ) ) ); - - return Output; - } - - ViconDataStreamSDK::CPP::Output_SetApexDeviceFeedback Client::SetApexDeviceFeedback( const String& i_rDeviceName, bool i_bOn ) - { - Output_SetApexDeviceFeedback Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->SetApexDeviceFeedback( i_rDeviceName, i_bOn ) ); - - return Output; - } - - // SetAxisMapping - CLASS_DECLSPEC - Output_SetAxisMapping Client::SetAxisMapping( const Direction::Enum XAxis, - const Direction::Enum YAxis, - const Direction::Enum ZAxis ) - { - Output_SetAxisMapping Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->SetAxisMapping( Adapt( XAxis ), - Adapt( YAxis ), - Adapt( ZAxis ) ) ); - - return Output; - } - - // GetAxisMapping - CLASS_DECLSPEC - Output_GetAxisMapping Client::GetAxisMapping() const - { - ViconDataStreamSDK::Core::Direction::Enum _XAxis; - ViconDataStreamSDK::Core::Direction::Enum _YAxis; - ViconDataStreamSDK::Core::Direction::Enum _ZAxis; - - m_pClientImpl->m_pCoreClient->GetAxisMapping( _XAxis, _YAxis, _ZAxis ); - - Output_GetAxisMapping Output; - Output.XAxis = Adapt( _XAxis ); - Output.YAxis = Adapt( _YAxis ); - Output.ZAxis = Adapt( _ZAxis ); - - return Output; - } - - ViconDataStreamSDK::CPP::Output_GetServerOrientation Client::GetServerOrientation() const - { - Output_GetServerOrientation Output; - ViconDataStreamSDK::Core::ServerOrientation::Enum Orientation; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetServerOrientation( Orientation ) ); - Output.Orientation = Adapt( Orientation ); - - return Output; - } - - // GetFrame - CLASS_DECLSPEC - Output_GetFrame Client::GetFrame() - { - Output_GetFrame Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetFrame() ); - - return Output; - } - - // GetFrameNumber - CLASS_DECLSPEC - Output_GetFrameNumber Client::GetFrameNumber() const - { - Output_GetFrameNumber Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetFrameNumber( Output.FrameNumber ) ); - - return Output; - } - - // GetTimecode - CLASS_DECLSPEC - Output_GetTimecode Client::GetTimecode() const - { - Output_GetTimecode Output; - ViconDataStreamSDK::Core::TimecodeStandard::Enum _TimecodeStandard; - - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetTimecode( Output.Hours, - Output.Minutes, - Output.Seconds, - Output.Frames, - Output.SubFrame, - Output.FieldFlag, - _TimecodeStandard, - Output.SubFramesPerFrame, - Output.UserBits ) ); - - Output.Standard = Adapt( _TimecodeStandard ); - - return Output; - } - - // GetFrameRate - CLASS_DECLSPEC - Output_GetFrameRate Client::GetFrameRate() const - { - Output_GetFrameRate Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetFrameRate( Output.FrameRateHz ) ); - - return Output; - } - - // GetLatencySampleCount - CLASS_DECLSPEC - Output_GetLatencySampleCount Client::GetLatencySampleCount() const - { - Output_GetLatencySampleCount Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetLatencySampleCount( Output.Count ) ); - - return Output; - } - - // GetLatencySampleName - CLASS_DECLSPEC - Output_GetLatencySampleName Client::GetLatencySampleName( const unsigned int LatencySampleIndex ) const - { - Output_GetLatencySampleName Output; - std::string Name; - - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetLatencySampleName( LatencySampleIndex, - Name ) ); - - Output.Name.Set( Name.c_str(), *m_pClientImpl->m_pStringFactory.get() ); - return Output; - } - - // GetLatencySampleValue - CLASS_DECLSPEC - Output_GetLatencySampleValue Client::GetLatencySampleValue( const String & LatencySampleName ) const - { - Output_GetLatencySampleValue Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetLatencySampleValue( LatencySampleName, - Output.Value ) ); - - return Output; - } - - // GetLatencyTotal - CLASS_DECLSPEC - Output_GetLatencyTotal Client::GetLatencyTotal() const - { - Output_GetLatencyTotal Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetLatencyTotal( Output.Total ) ); - - return Output; - } - - // GetHardwareFrameNumber - CLASS_DECLSPEC - Output_GetHardwareFrameNumber Client::GetHardwareFrameNumber() const - { - Output_GetHardwareFrameNumber Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetHardwareFrameNumber( Output.HardwareFrameNumber ) ); - - return Output; - } - - Output_GetFrameRateCount Client::GetFrameRateCount() const - { - Output_GetFrameRateCount Output; - Output.Result = Adapt ( m_pClientImpl->m_pCoreClient->GetFrameRateCount( Output.Count ) ); - - return Output; - } - - Output_GetFrameRateName Client::GetFrameRateName( const unsigned int FrameRateIndex ) const - { - Output_GetFrameRateName Output; - std::string Name; - - Output.Result = Adapt ( m_pClientImpl->m_pCoreClient->GetFrameRateName( FrameRateIndex, Name ) ); - - Output.Name.Set( Name.c_str(), *m_pClientImpl->m_pStringFactory.get() ); - return Output; - } - - Output_GetFrameRateValue Client::GetFrameRateValue( const String & FrameRateName ) const - { - Output_GetFrameRateValue Output; - Output.Result = Adapt ( m_pClientImpl->m_pCoreClient->GetFrameRateValue( FrameRateName, Output.Value ) ); - - return Output; - } - - // GetSubjectCount - CLASS_DECLSPEC - Output_GetSubjectCount Client::GetSubjectCount() const - { - Output_GetSubjectCount Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetSubjectCount( Output.SubjectCount ) ); - - return Output; - } - - // GetSubjectName - CLASS_DECLSPEC - Output_GetSubjectName Client::GetSubjectName( const unsigned int SubjectIndex ) const - { - Output_GetSubjectName Output; - - std::string SubjectName; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetSubjectName( SubjectIndex, SubjectName ) ); - Output.SubjectName.Set( SubjectName.c_str(), *m_pClientImpl->m_pStringFactory.get() ); - - return Output; - } - - // GetSubjectRootSegmentName - CLASS_DECLSPEC - Output_GetSubjectRootSegmentName Client::GetSubjectRootSegmentName( const String & SubjectName ) const - { - Output_GetSubjectRootSegmentName Output; - std::string SegmentName; - - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetSubjectRootSegmentName( SubjectName, SegmentName ) ); - - Output.SegmentName.Set( SegmentName.c_str(), *m_pClientImpl->m_pStringFactory.get() ); - return Output; - } - - // GetSegmentCount - CLASS_DECLSPEC - Output_GetSegmentCount Client::GetSegmentCount( const String & SubjectName ) const - { - Output_GetSegmentCount Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetSegmentCount( SubjectName, Output.SegmentCount ) ); - - return Output; - } - - // GetSegmentName - CLASS_DECLSPEC - Output_GetSegmentName Client::GetSegmentName( const String & SubjectName, - const unsigned int SegmentIndex ) const - { - Output_GetSegmentName Output; - std::string SegmentName; - - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetSegmentName( SubjectName, SegmentIndex, SegmentName ) ); - - Output.SegmentName.Set( SegmentName.c_str(), *m_pClientImpl->m_pStringFactory.get() ); - return Output; - } - - // GetSegmentChildCount - CLASS_DECLSPEC - Output_GetSegmentChildCount Client::GetSegmentChildCount( const String & SubjectName, - const String & SegmentName ) const - { - Output_GetSegmentChildCount Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetSegmentChildCount( SubjectName, SegmentName, Output.SegmentCount ) ); - - return Output; - } - - // GetSegmentChildName - CLASS_DECLSPEC - Output_GetSegmentChildName Client::GetSegmentChildName( const String & SubjectName, - const String & SegmentName, - const unsigned int SegmentIndex ) const - { - Output_GetSegmentChildName Output; - std::string SegmentChildName; - - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetSegmentChildName( SubjectName, SegmentName, SegmentIndex, SegmentChildName ) ); - - Output.SegmentName.Set( SegmentChildName.c_str(), *m_pClientImpl->m_pStringFactory.get() ); - return Output; - } - - // GetSegmentParentName - CLASS_DECLSPEC - Output_GetSegmentParentName Client::GetSegmentParentName( const String & SubjectName, - const String & SegmentName ) const - { - Output_GetSegmentParentName Output; - std::string SegmentParentName; - - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetSegmentParentName( SubjectName, SegmentName, SegmentParentName ) ); - - Output.SegmentName.Set( SegmentParentName.c_str(), *m_pClientImpl->m_pStringFactory.get() ); - return Output; - } - - // GetSegmentGlobalTranslation - CLASS_DECLSPEC - Output_GetSegmentGlobalTranslation Client::GetSegmentGlobalTranslation( const String & SubjectName, - const String & SegmentName ) const - { - Output_GetSegmentGlobalTranslation Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetSegmentGlobalTranslation( SubjectName, - SegmentName, - Output.Translation, - Output.Occluded ) ); - - return Output; - } - - // GetSegmentGlobalRotationHelical - CLASS_DECLSPEC - Output_GetSegmentGlobalRotationHelical Client::GetSegmentGlobalRotationHelical( const String & SubjectName, - const String & SegmentName ) const - { - Output_GetSegmentGlobalRotationHelical Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetSegmentGlobalRotationHelical( SubjectName, - SegmentName, - Output.Rotation, - Output.Occluded ) ); - - return Output; - } - - // GetSegmentGlobalRotationMatrix - CLASS_DECLSPEC - Output_GetSegmentGlobalRotationMatrix Client::GetSegmentGlobalRotationMatrix( const String & SubjectName, - const String & SegmentName ) const - { - Output_GetSegmentGlobalRotationMatrix Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetSegmentGlobalRotationMatrix( SubjectName, - SegmentName, - Output.Rotation, - Output.Occluded ) ); - - return Output; - } - - // GetSegmentGlobalRotationQuaternion - CLASS_DECLSPEC - Output_GetSegmentGlobalRotationQuaternion Client::GetSegmentGlobalRotationQuaternion( const String & SubjectName, - const String & SegmentName ) const - { - Output_GetSegmentGlobalRotationQuaternion Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetSegmentGlobalRotationQuaternion( SubjectName, - SegmentName, - Output.Rotation, - Output.Occluded ) ); - - return Output; - } - - // GetSegmentGlobalRotationEulerXYZ - CLASS_DECLSPEC - Output_GetSegmentGlobalRotationEulerXYZ Client::GetSegmentGlobalRotationEulerXYZ( const String & SubjectName, - const String & SegmentName ) const - { - Output_GetSegmentGlobalRotationEulerXYZ Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetSegmentGlobalRotationEulerXYZ( SubjectName, - SegmentName, - Output.Rotation, - Output.Occluded ) ); - - return Output; - } - - // GetSegmentStaticTranslation - CLASS_DECLSPEC - Output_GetSegmentStaticTranslation Client::GetSegmentStaticTranslation( const String & SubjectName, - const String & SegmentName ) const - { - Output_GetSegmentStaticTranslation Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetSegmentStaticTranslation( SubjectName, - SegmentName, - Output.Translation ) ); - - return Output; - } - - // GetSegmentStaticRotationHelical - CLASS_DECLSPEC - Output_GetSegmentStaticRotationHelical Client::GetSegmentStaticRotationHelical( const String & SubjectName, - const String & SegmentName ) const - { - Output_GetSegmentStaticRotationHelical Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetSegmentStaticRotationHelical( SubjectName, - SegmentName, - Output.Rotation ) ); - - return Output; - } - - // GetSegmentStaticRotationMatrix - CLASS_DECLSPEC - Output_GetSegmentStaticRotationMatrix Client::GetSegmentStaticRotationMatrix( const String & SubjectName, - const String & SegmentName ) const - { - Output_GetSegmentStaticRotationMatrix Output; - Output.Result = Adapt(m_pClientImpl->m_pCoreClient->GetSegmentStaticRotationMatrix(SubjectName, - SegmentName, - Output.Rotation)); - - return Output; - } - - // GetSegmentStaticRotationQuaternion - CLASS_DECLSPEC - Output_GetSegmentStaticRotationQuaternion Client::GetSegmentStaticRotationQuaternion( const String & SubjectName, - const String & SegmentName ) const - { - Output_GetSegmentStaticRotationQuaternion Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetSegmentStaticRotationQuaternion( SubjectName, - SegmentName, - Output.Rotation ) ); - - return Output; - } - - // GetSegmentStaticRotationEulerXYZ - CLASS_DECLSPEC - Output_GetSegmentStaticRotationEulerXYZ Client::GetSegmentStaticRotationEulerXYZ( const String & SubjectName, - const String & SegmentName ) const - { - Output_GetSegmentStaticRotationEulerXYZ Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetSegmentStaticRotationEulerXYZ( SubjectName, - SegmentName, - Output.Rotation ) ); - - return Output; - } - - // GetSegmentLocalTranslation - CLASS_DECLSPEC - Output_GetSegmentLocalTranslation Client::GetSegmentLocalTranslation( const String & SubjectName, - const String & SegmentName ) const - { - Output_GetSegmentLocalTranslation Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetSegmentLocalTranslation( SubjectName, - SegmentName, - Output.Translation, - Output.Occluded ) ); - - return Output; - } - - // GetSegmentLocalRotationHelical - CLASS_DECLSPEC - Output_GetSegmentLocalRotationHelical Client::GetSegmentLocalRotationHelical( const String & SubjectName, - const String & SegmentName ) const - { - Output_GetSegmentLocalRotationHelical Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetSegmentLocalRotationHelical( SubjectName, - SegmentName, - Output.Rotation, - Output.Occluded ) ); - - return Output; - } - - // GetSegmentLocalRotationMatrix - CLASS_DECLSPEC - Output_GetSegmentLocalRotationMatrix Client::GetSegmentLocalRotationMatrix( const String & SubjectName, - const String & SegmentName ) const - { - Output_GetSegmentLocalRotationMatrix Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetSegmentLocalRotationMatrix( SubjectName, - SegmentName, - Output.Rotation, - Output.Occluded ) ); - - return Output; - } - - // GetSegmentLocalRotationQuaternion - CLASS_DECLSPEC - Output_GetSegmentLocalRotationQuaternion Client::GetSegmentLocalRotationQuaternion( const String & SubjectName, - const String & SegmentName ) const - { - Output_GetSegmentLocalRotationQuaternion Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetSegmentLocalRotationQuaternion( SubjectName, - SegmentName, - Output.Rotation, - Output.Occluded ) ); - - return Output; - } - - // GetSegmentLocalRotationEulerXYZ - CLASS_DECLSPEC - Output_GetSegmentLocalRotationEulerXYZ Client::GetSegmentLocalRotationEulerXYZ( const String & SubjectName, - const String & SegmentName ) const - { - Output_GetSegmentLocalRotationEulerXYZ Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetSegmentLocalRotationEulerXYZ( SubjectName, - SegmentName, - Output.Rotation, - Output.Occluded ) ); - - return Output; - } - - // GetObjectQuality - CLASS_DECLSPEC - Output_GetObjectQuality Client::GetObjectQuality( const String & ObjectName ) const - { - Output_GetObjectQuality Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetObjectQuality( ObjectName, Output.Quality ) ); - return Output; - } - - // GetMarkerCount - CLASS_DECLSPEC - Output_GetMarkerCount Client::GetMarkerCount( const String & SubjectName ) const - { - Output_GetMarkerCount Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetMarkerCount( SubjectName, Output.MarkerCount ) ); - - return Output; - } - - // GetMarkerName - CLASS_DECLSPEC - Output_GetMarkerName Client::GetMarkerName( const String & SubjectName, - const unsigned int MarkerIndex ) const - { - Output_GetMarkerName Output; - std::string MarkerName; - - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetMarkerName( SubjectName, MarkerIndex, MarkerName ) ); - - Output.MarkerName.Set( MarkerName.c_str(), *m_pClientImpl->m_pStringFactory.get() ); - return Output; - } - - // GetMarkerParentName - CLASS_DECLSPEC - Output_GetMarkerParentName Client::GetMarkerParentName( const String & SubjectName, - const String & MarkerName ) const - { - Output_GetMarkerParentName Output; - std::string MarkerParentName; - - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetMarkerParentName( SubjectName, MarkerName, MarkerParentName ) ); - - Output.SegmentName.Set( MarkerParentName.c_str(), *m_pClientImpl->m_pStringFactory.get() ); - return Output; - } - - // GetMarkerGlobalTranslation - CLASS_DECLSPEC - Output_GetMarkerGlobalTranslation Client::GetMarkerGlobalTranslation( const String & SubjectName, - const String & MarkerName ) const - { - Output_GetMarkerGlobalTranslation Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetMarkerGlobalTranslation( SubjectName, - MarkerName, - Output.Translation, - Output.Occluded ) ); - - return Output; - } - - // GetMarkerRayContributionCount - CLASS_DECLSPEC - ViconDataStreamSDK::CPP::Output_GetMarkerRayContributionCount Client::GetMarkerRayContributionCount( const String & SubjectName, const String & MarkerName ) const - { - Output_GetMarkerRayContributionCount Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetMarkerRayAssignmentCount( SubjectName, MarkerName, Output.RayContributionsCount ) ); - - return Output; - } - - // GetMarkerRayContribution - CLASS_DECLSPEC - ViconDataStreamSDK::CPP::Output_GetMarkerRayContribution Client::GetMarkerRayContribution( const String & SubjectName, const String & MarkerName, unsigned int MarkerRayContributionIndex ) const - { - Output_GetMarkerRayContribution Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetMarkerRayAssignment( SubjectName, MarkerName, MarkerRayContributionIndex, Output.CameraID, Output.CentroidIndex ) ); - - return Output; - } - - // GetUnlabeledMarkerCount - CLASS_DECLSPEC - Output_GetUnlabeledMarkerCount Client::GetUnlabeledMarkerCount() const - { - Output_GetUnlabeledMarkerCount Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetUnlabeledMarkerCount( Output.MarkerCount ) ); - - return Output; - } - - // GetUnlabeledMarkerGlobalTranslation - CLASS_DECLSPEC - Output_GetUnlabeledMarkerGlobalTranslation Client::GetUnlabeledMarkerGlobalTranslation( const unsigned int MarkerIndex ) const - { - Output_GetUnlabeledMarkerGlobalTranslation Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetUnlabeledMarkerGlobalTranslation( MarkerIndex, Output.Translation ) ); - - return Output; - } - - // GetLabeledMarkerCount - CLASS_DECLSPEC - Output_GetLabeledMarkerCount Client::GetLabeledMarkerCount() const - { - Output_GetLabeledMarkerCount Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetLabeledMarkerCount( Output.MarkerCount ) ); - - return Output; - } - - // GetLabeledMarkerGlobalTranslation - CLASS_DECLSPEC - Output_GetLabeledMarkerGlobalTranslation Client::GetLabeledMarkerGlobalTranslation( const unsigned int MarkerIndex ) const - { - Output_GetLabeledMarkerGlobalTranslation Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetLabeledMarkerGlobalTranslation( MarkerIndex, Output.Translation ) ); - - return Output; - } - - // GetDeviceCount - CLASS_DECLSPEC - Output_GetDeviceCount Client::GetDeviceCount() const - { - Output_GetDeviceCount Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetDeviceCount( Output.DeviceCount ) ); - - return Output; - } - - // GetDeviceName - CLASS_DECLSPEC - Output_GetDeviceName Client::GetDeviceName( const unsigned int DeviceIndex ) const - { - Output_GetDeviceName Output; - std::string _DeviceName; - ViconDataStreamSDK::Core::DeviceType::Enum _DeviceType; - - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetDeviceName( DeviceIndex, - _DeviceName, - _DeviceType ) ); - - Output.DeviceName.Set( _DeviceName.c_str(),* m_pClientImpl->m_pStringFactory.get() ); - Output.DeviceType = Adapt( _DeviceType ); - return Output; - } - - // GetDeviceOutputCount - CLASS_DECLSPEC - Output_GetDeviceOutputCount Client::GetDeviceOutputCount( const String & DeviceName ) const - { - Output_GetDeviceOutputCount Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetDeviceOutputCount( DeviceName, Output.DeviceOutputCount ) ); - - return Output; - } - - // GetDeviceOutputName - CLASS_DECLSPEC - Output_GetDeviceOutputName Client::GetDeviceOutputName( const String & DeviceName, - const unsigned int DeviceOutputIndex ) const - { - Output_GetDeviceOutputName Output; - std::string _DeviceOutputName; - ViconDataStreamSDK::Core::Unit::Enum _DeviceOutputUnit; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetDeviceOutputName( DeviceName, - DeviceOutputIndex, - _DeviceOutputName, - _DeviceOutputUnit ) ); - - Output.DeviceOutputName.Set( _DeviceOutputName.c_str(), *m_pClientImpl->m_pStringFactory.get() ); - Output.DeviceOutputUnit = Adapt( _DeviceOutputUnit ); - - return Output; - } - - // GetDeviceOutputValue - CLASS_DECLSPEC - Output_GetDeviceOutputValue Client::GetDeviceOutputValue( const String & DeviceName, - const String & DeviceOutputName ) const - { - Output_GetDeviceOutputValue Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetDeviceOutputValue( DeviceName, - DeviceOutputName, - Output.Value, - Output.Occluded ) ); - - return Output; - } - - // GetDeviceOutputSubsamples - CLASS_DECLSPEC - Output_GetDeviceOutputSubsamples Client::GetDeviceOutputSubsamples( const String & DeviceName, - const String & DeviceOutputName ) const - { - Output_GetDeviceOutputSubsamples Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetDeviceOutputSubsamples( DeviceName, - DeviceOutputName, - Output.DeviceOutputSubsamples, - Output.Occluded ) ); - - return Output; - } - - // GetDeviceOutputValue - CLASS_DECLSPEC - Output_GetDeviceOutputValue Client::GetDeviceOutputValue( const String & DeviceName, - const String & DeviceOutputName, - const unsigned int Subsample ) const - { - Output_GetDeviceOutputValue Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetDeviceOutputValue( DeviceName, - DeviceOutputName, - Subsample, - Output.Value, - Output.Occluded ) ); - - return Output; - } - - // GetForcePlateCount - CLASS_DECLSPEC - Output_GetForcePlateCount Client::GetForcePlateCount() const - { - Output_GetForcePlateCount Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetForcePlateCount( Output.ForcePlateCount ) ); - - return Output; - } - - // GetGlobalForceVector - CLASS_DECLSPEC - Output_GetGlobalForceVector Client::GetGlobalForceVector( const unsigned int ForcePlateIndex ) const - { - return GetGlobalForceVector( ForcePlateIndex, 0 ); - } - - // GetGlobalMomentVector - CLASS_DECLSPEC - Output_GetGlobalMomentVector Client::GetGlobalMomentVector( const unsigned int ForcePlateIndex ) const - { - return GetGlobalMomentVector( ForcePlateIndex, 0 ); - } - - // GetGlobalCentreOfPressure - CLASS_DECLSPEC - Output_GetGlobalCentreOfPressure Client::GetGlobalCentreOfPressure( const unsigned int ForcePlateIndex ) const - { - return GetGlobalCentreOfPressure( ForcePlateIndex, 0 ); - } - - // ForcePlateSubsamples - CLASS_DECLSPEC - Output_GetForcePlateSubsamples Client::GetForcePlateSubsamples( const unsigned int ForcePlateIndex ) const - { - Output_GetForcePlateSubsamples Output; - Output.ForcePlateSubsamples = 0; - - unsigned int ForcePlateID; - - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetForcePlateID( ForcePlateIndex, ForcePlateID ) ); - - if( Output.Result == Result::Success ) - { - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetForcePlateSubsamples( ForcePlateID, Output.ForcePlateSubsamples ) ); - } - - return Output; - } - - // GetGlobalForceVector - CLASS_DECLSPEC - Output_GetGlobalForceVector Client::GetGlobalForceVector( const unsigned int ForcePlateIndex, const unsigned int Subsample ) const - { - Output_GetGlobalForceVector Output; - unsigned int ForcePlateID; - - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetForcePlateID( ForcePlateIndex, ForcePlateID ) ); - - if( Output.Result == Result::Success ) - { - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetGlobalForceVector( ForcePlateID, Subsample, Output.ForceVector ) ); - } - else - { - Output.ForceVector[ 0 ] = 0.0; - Output.ForceVector[ 1 ] = 0.0; - Output.ForceVector[ 2 ] = 0.0; - } - - return Output; - } - - // GetGlobalMomentVector - CLASS_DECLSPEC - Output_GetGlobalMomentVector Client::GetGlobalMomentVector( const unsigned int ForcePlateIndex, const unsigned int Subsample ) const - { - Output_GetGlobalMomentVector Output; - unsigned int ForcePlateID; - - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetForcePlateID( ForcePlateIndex, ForcePlateID ) ); - - if( Output.Result == Result::Success ) - { - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetGlobalMomentVector( ForcePlateID, Subsample, Output.MomentVector ) ); - } - else - { - Output.MomentVector[ 0 ] = 0.0; - Output.MomentVector[ 1 ] = 0.0; - Output.MomentVector[ 2 ] = 0.0; - } - - return Output; - } - - // GetGlobalCentreOfPressure - CLASS_DECLSPEC - Output_GetGlobalCentreOfPressure Client::GetGlobalCentreOfPressure( const unsigned int ForcePlateIndex, const unsigned int Subsample ) const - { - Output_GetGlobalCentreOfPressure Output; - unsigned int ForcePlateID; - - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetForcePlateID( ForcePlateIndex, ForcePlateID ) ); - - if( Output.Result == Result::Success ) - { - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetGlobalCentreOfPressure( ForcePlateID, Subsample, Output.CentreOfPressure ) ); - } - else - { - Output.CentreOfPressure[ 0 ] = 0.0; - Output.CentreOfPressure[ 1 ] = 0.0; - Output.CentreOfPressure[ 2 ] = 0.0; - } - - return Output; - } - - - // GetEyeTrackerCount - CLASS_DECLSPEC - Output_GetEyeTrackerCount Client::GetEyeTrackerCount() const - { - Output_GetEyeTrackerCount Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetEyeTrackerCount( Output.EyeTrackerCount ) ); - - return Output; - } - - // GetEyeTrackerGlobalPosition - CLASS_DECLSPEC - Output_GetEyeTrackerGlobalPosition Client::GetEyeTrackerGlobalPosition( const unsigned int EyeTrackerIndex ) const - { - Output_GetEyeTrackerGlobalPosition Output; - unsigned int EyeTrackerID; - - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetEyeTrackerID( EyeTrackerIndex, EyeTrackerID ) ); - - if( Output.Result == Result::Success ) - { - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetEyeTrackerGlobalPosition( EyeTrackerID, Output.Position, Output.Occluded ) ); - } - else - { - Output.Position[ 0 ] = 0.0; - Output.Position[ 1 ] = 0.0; - Output.Position[ 2 ] = 0.0; - Output.Occluded = false; - } - - return Output; - } - - // GetEyeTrackerGlobalGazeVector - CLASS_DECLSPEC - Output_GetEyeTrackerGlobalGazeVector Client::GetEyeTrackerGlobalGazeVector( const unsigned int EyeTrackerIndex ) const - { - Output_GetEyeTrackerGlobalGazeVector Output; - unsigned int EyeTrackerID; - - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetEyeTrackerID( EyeTrackerIndex, EyeTrackerID ) ); - - if( Output.Result == Result::Success ) - { - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetEyeTrackerGlobalGazeVector( EyeTrackerID, Output.GazeVector, Output.Occluded ) ); - } - else - { - Output.GazeVector[ 0 ] = 0.0; - Output.GazeVector[ 1 ] = 0.0; - Output.GazeVector[ 2 ] = 0.0; - Output.Occluded = false; - } - - return Output; - } - - CLASS_DECLSPEC - Output_GetCameraCount Client::GetCameraCount() const - { - Output_GetCameraCount Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetCameraCount( Output.CameraCount ) ); - - return Output; - } - - CLASS_DECLSPEC - Output_GetCameraName Client::GetCameraName( unsigned int i_CameraIndex ) const - { - Output_GetCameraName Output; - std::string _CameraName; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetCameraName( i_CameraIndex, _CameraName ) ); - Output.CameraName.Set( _CameraName.c_str(),* m_pClientImpl->m_pStringFactory.get() ); - - return Output; - } - - CLASS_DECLSPEC - Output_GetCameraId Client::GetCameraId( const std::string & i_rCameraName ) const - { - Output_GetCameraId Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetCameraID( i_rCameraName, Output.CameraId ) ); - - return Output; - } - - CLASS_DECLSPEC - Output_GetCameraUserId Client::GetCameraUserId( const std::string & i_rCameraName ) const - { - Output_GetCameraUserId Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetCameraUserID( i_rCameraName, Output.CameraUserId ) ); - - return Output; - } - - CLASS_DECLSPEC - Output_GetCameraType Client::GetCameraType( const std::string & i_rCameraName ) const - { - Output_GetCameraType Output; - std::string _CameraType; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetCameraType( i_rCameraName, _CameraType ) ); - Output.CameraType.Set( _CameraType.c_str(),* m_pClientImpl->m_pStringFactory.get() ); - - return Output; - } - - CLASS_DECLSPEC - Output_GetCameraDisplayName Client::GetCameraDisplayName( const std::string & i_rCameraName ) const - { - Output_GetCameraDisplayName Output; - std::string _CameraDisplayName; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetCameraDisplayName( i_rCameraName, _CameraDisplayName ) ); - Output.CameraDisplayName.Set( _CameraDisplayName.c_str(),* m_pClientImpl->m_pStringFactory.get() ); - - return Output; - } - - CLASS_DECLSPEC - Output_GetCameraResolution Client::GetCameraResolution( const std::string & i_rCameraName ) const - { - Output_GetCameraResolution Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetCameraResolution( i_rCameraName, Output.ResolutionX, Output.ResolutionY ) ); - - return Output; - } - - CLASS_DECLSPEC - Output_GetIsVideoCamera Client::GetIsVideoCamera( const std::string & i_rCameraName ) const - { - Output_GetIsVideoCamera Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetIsVideoCamera( i_rCameraName, Output.IsVideoCamera ) ); - - return Output; - } - - CLASS_DECLSPEC - Output_GetCentroidCount Client::GetCentroidCount( const std::string & i_rCameraName ) const - { - Output_GetCentroidCount Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetCentroidCount( i_rCameraName, Output.CentroidCount ) ); - - return Output; - } - - CLASS_DECLSPEC - Output_GetCentroidPosition Client::GetCentroidPosition( const std::string & i_rCameraName, const unsigned int i_CentroidIndex ) const - { - Output_GetCentroidPosition Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetCentroidPosition( i_rCameraName, i_CentroidIndex, Output.CentroidPosition, Output.Radius /*, Output.Accuracy */ ) ); - - return Output; - } - - CLASS_DECLSPEC - Output_GetCentroidWeight Client::GetCentroidWeight( const std::string & i_rCameraName, const unsigned int i_CentroidIndex ) const - { - Output_GetCentroidWeight Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetCentroidWeight( i_rCameraName, i_CentroidIndex, Output.Weight ) ); - - return Output; - } - - CLASS_DECLSPEC - Output_GetGreyscaleBlobCount Client::GetGreyscaleBlobCount( const std::string & i_rCameraName ) const - { - Output_GetGreyscaleBlobCount Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetGreyscaleBlobCount( i_rCameraName, Output.BlobCount ) ); - - return Output; - } - - CLASS_DECLSPEC - Output_GetGreyscaleBlob Client::GetGreyscaleBlob( const std::string & i_rCameraName, const unsigned int i_BlobIndex ) const - { - Output_GetGreyscaleBlob Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetGreyscaleBlob( i_rCameraName, i_BlobIndex, Output.BlobLinePositionsX, Output.BlobLinePositionsY, Output.BlobLinePixelValues ) ); - - return Output; - } - - CLASS_DECLSPEC - Output_GetVideoFrame Client::GetVideoFrame( const std::string & i_rCameraName ) const - { - - ViconCGStreamClientSDK::VVideoFramePtr VideoFramePtr; - - Output_GetVideoFrame Output; - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->GetVideoFrame( i_rCameraName, VideoFramePtr ) ); - if ( Output.Result == Result::Enum::Success ) - { - Output.m_FrameID = (*VideoFramePtr).m_FrameID; - Output.m_Format = (*VideoFramePtr).m_Format; - Output.m_Width = (*VideoFramePtr).m_Width; - Output.m_Height = (*VideoFramePtr).m_Height; - - Output.m_Data.reset( new std::vector< unsigned char >( (*VideoFramePtr).m_VideoData.size() ) ); - try - { - *Output.m_Data = (*VideoFramePtr).m_VideoData; - } - catch ( const std::length_error & ) - { - // Problem rerieving the data from the CG stream - Output.Result = Result::Unknown; - } - } - - return Output; - } - - CLASS_DECLSPEC - Output_SetCameraFilter Client::SetCameraFilter( const std::vector< unsigned int > & i_rCameraIdsForCentroids, - const std::vector< unsigned int > & i_rCameraIdsForBlobs, - const std::vector< unsigned int > & i_rCameraIdsForVideo ) - { - Output_SetCameraFilter Output; - - Output.Result = Adapt( m_pClientImpl->m_pCoreClient->SetCameraFilter( i_rCameraIdsForCentroids, i_rCameraIdsForBlobs, i_rCameraIdsForVideo ) ); - return Output; - } - -} // End of namespace CPP -} // End of namespace ViconDataStreamSDK diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/DataStreamClient.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/DataStreamClient.h deleted file mode 100644 index 2701ff10a..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/DataStreamClient.h +++ /dev/null @@ -1,6692 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// The SDK client and supporting definitions. - -#include "IDataStreamClientBase.h" - -#include -#include -#include - -namespace ViconDataStreamSDK -{ -namespace CPP -{ - - class Output_EnableVideoData : public Output_SimpleResult {}; - class Output_DisableVideoData : public Output_SimpleResult {}; - class Output_IsVideoDataEnabled : public Output_EnabledFlag {}; - class Output_GetVideoFrame - { - public: - Result::Enum Result; - - enum EFormat { ENoVideo = 0, EMono8, EBayerRG8, EBayerGB8, EBayerGR8, EBayerBG8, ERGB888, EBGR888 }; - - unsigned int m_FrameID; - unsigned short m_Width; - unsigned short m_Height; - unsigned int m_Format; - std::shared_ptr< std::vector< unsigned char > > m_Data; - }; - - class ClientImpl; - - /// Vicon DataStream SDK client. - /// The core client class for C++. - class CLASS_DECLSPEC Client : public IDataStreamClientBase - { - public: - /// Construction. - /// You can create many instances of the Vicon DataStream Client which can connect to multiple Vicon DataStream Servers. - /// - /// - /// C example - /// - /// // The C version uses explicit creation methods - /// - /// CClient * pClient = ClientCreate(); - /// // C Client functions take the client as a parameter - /// CBool ok = Client_SomeFunction( pClient, Args ); - /// // The C client needs to be explicitly destroyed - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// - /// // C++ version of the SDK is object oriented, so use the class constructor. - /// - /// ViconDataStreamSDK::CPP::Client StackClient; - /// Output_SomeFunction Output = StackClient.SomeFunction(); - /// // Client is implicitly destroyed as it goes out of scope. - /// - /// // Alternatively the Client can be made on the heap. - /// - /// ViconDataStreamSDK::CPP::Client * pHeapClient - /// = new ViconDataStreamSDK::CPP::Client(); - /// Output_SomeFunction Output = pHeapClient->SomeFunction( Input ); - /// delete pHeapClient; - /// - /// MATLAB example - /// - /// %% The MATLAB SDK is object oriented, and needs to be explicitly loaded - /// and unloaded. - /// - /// Client.LoadViconDataStreamSDK(); - /// pHeapClient = Client(); - /// Output = pHeapClient.SomeFunction(Input); - /// Client.UnloadViconDataStreamSDK(); - /// - /// .NET example - /// - /// ///.NET is object oriented, so use the class constructor. Because objects are - /// // lazily garbage collected, your instance may outlive the last reference to it - /// // for some time.If the instance is pre - fetching frame data for you, then it - /// // can still use CPU and network bandwidth.Consider explicitly disconnecting - /// // prior to destruction. - /// - /// ViconDataStreamSDK.DotNET.Client pHeapClient = new ViconDataStreamSDK.DotNET.Client(); - /// Output_SomeFunction Output = pHeapClient.SomeFunction(InputParam); - /// // Signal to the garbage collector that it can clean up pHeapClient.Disconnect(); - /// pHeapClient = null; - /// ----- - /// - Client(); - - /// Destruction. - /// Destruction will Disconnect if required. - /// - /// See Client::Client for an example. - virtual ~Client(); - - /// Get the version of the Vicon DataStream SDK. - /// + **Major** - /// When this number increases, we break backward compatibility with previous major versions. - /// + **Minor** - /// When this number increases, we have probably added new functionality to the SDK without breaking - /// backward compatibility with previous versions. - /// + **Point** - /// When this number increases, we have introduced a bug fix or performance enhancement without breaking - /// backward compatibility with previous versions. - /// - /// The function can be called without the client being connected. - /// - /// - /// C example - /// - /// - /// CClient * pClient = Client_Create(); - /// COutput_GetVersion Output = Client_GetVersion( pClient ); - /// Client_Destroy( pClient ); - /// - /// - /// C++ example - /// - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// Output_GetVersion Output = MyClient.GetVersion(); - /// - /// - /// MATLAB example - /// - /// - /// MyClient = Client(); - /// Output = MyClient.GetVersion(); - /// - /// - /// .NET example - /// - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// Output_GetVersion Output = MyClient.GetVersion(); - /// ----- - /// - /// \return Output_GetVersion class containing the version information. - Output_GetVersion GetVersion() const; - - - /// Establish a dedicated connection to a Vicon DataStream Server. - /// - /// See Also: ConnectToMulticast(), Disconnect(), IsConnected(). - /// - /// The function defaults to connecting on port 801. - /// You can specify an alternate port number after a colon. - /// This is for future compatibility: current products serve data on port 801 only. - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// COutput_Connect Output = Client_Connect( pClient, "localhost"); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// Output_Connect Output = MyClient.Connect( "localhost" ); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// Output = MyClient.Connect('locahost:801'); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// Output_Connect Output = MyClient.Connect("localhost:801"); - /// ----- - /// - /// \param HostName The DNS-identifiable name, or IP address of the PC hosting the DataStream server. - /// For example: - /// + "localhost" - /// + "MyViconPC:801" - /// + "10.0.0.2" - /// \return An Output_Connect class containing the result of the connect operation. - /// - The Result will be: - /// + Success - /// + InvalidHostName - /// + ClientAlreadyConnected - /// + ClientConnectionFailed - Output_Connect Connect( const String & HostName ); - - /// Connect to a Vicon DataStream Server's Multicast stream. - /// The stream content is managed by a client who calls StartTransmittingMulticast(). - /// - /// See Also: Connect(), Disconnect(), IsConnected(), StartTransmittingMulticast(), StopTransmittingMulticast() - /// - /// // class Output_ConnectToMulticast - /// // { - /// // public: - /// // Result::Enum Result; - /// // }; - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// COutput_Connect Output = Client_ConnectToMulticast( pClient, "localhost", "224.0.0.0" ); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// Output_ConnectToMulticast Output = MyClient.ConnectToMulticast( "localhost", "224.0.0.0" ); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// Output = MyClient.ConnectToMulticast('locahost', '224.0.0.0'); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client();; - /// Output_ConnectToMulticast Output = MyClient.ConnectToMulticast("localhost", "224.0.0.0"); - /// ----- - /// - /// - /// \param LocalIP The DNS-identifiable name, or IP address of the local Ethernet interface - /// on which you wish to receive multicast data. - /// Do not specify a port (any port specified will be ignored). - /// For example: - /// + "localhost" - /// + "10.0.0.2" - /// \param MulticastIP The IP Address of the Multicast group on which data will be received. - /// The address must be in the range 224.0.0.0-239.255.255.255 - /// You may also specify a port by appending it to the end of the IP Address - /// after a colon, e.g. 224.0.0.0:30001. - /// If you do not specify a port it will default to 44801. - /// \return An Output_ConnectToMulticast class containing the result of the connect operation. - /// - The Result will be: - /// + Success - /// + InvalidHostName - /// + InvalidMulticastIP - /// + ClientAlreadyConnected - /// + ClientConnectionFailed - Output_ConnectToMulticast ConnectToMulticast( const String & LocalIP, const String & MulticastIP ); - - /// Disconnect from the Vicon DataStream Server. - /// - /// See Also: Connect(), IsConnected() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// COutput_Disconnect Output = Client_Disconnect( pClient ); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_Disconnect Output = MyClient.Disconnect(); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect("localhost"); - /// Output = MyClient.Disconnect() - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect("localhost"); - /// Output_Disconnect Output = MyClient.Disconnect(); - /// ----- - /// - /// \return An Output_Disconnect class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - Output_Disconnect Disconnect(); - - /// Discover whether client is connected to the Vicon DataStream Server. - /// - /// See Also: Connect(), Disconnect() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// CBool Output = Client_IsConnected( pClient ); - /// // Output == 0 - /// Client_Connect( pClient, "localhost" ); - /// Output = Client_IsConnected( pClient ); - /// // Output == 1 - /// COutput_Disconnect Output = Client_Disconnect( pClient ); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// Output_IsConnected Output = MyClient.IsConnected() - /// // Output.Connected == false - /// MyClient.Connect( "localhost" ); - /// Output_IsConnected Output = MyClient.IsConnected() - /// // Output.Connected == true - /// // (assuming localhost is serving) - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// Output_IsConnected Output = MyClient.IsConnected() - /// // Output.Connected == false - /// MyClient.Connect( "localhost" ); - /// Output_IsConnected Output = MyClient.IsConnected() - /// // Output.Connected == true - /// // (assuming localhost is serving) - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// Output_IsConnected Output = MyClient.IsConnected() - /// // Output.Connected == false - /// MyClient.Connect( "localhost" ); - /// Output_IsConnected Output = MyClient.IsConnected() - /// // Output.Connected == true - /// // (assuming localhost is serving) - /// ----- - /// - /// \return An Output_IsConnected class containing a true value for Connected if you are - /// connected to the stream, otherwise false. - Output_IsConnected IsConnected() const; - - /// Ask the DataStream Server to start transmitting the data you are receiving directly to a Multicast address as well. - /// This allows multiple clients to connect to your stream (via ConnectToMulticast() ) whilst minimizing network - /// bandwidth use and frame delivery latency. - /// - /// See Also: Connect(), ConnectToMulticast(), Disconnect(), StopTransmittingMulticast() - /// - - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_StartTransmittingMulticast( pClient, "10.0.0.1", "224.0.0.0" ); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.StartTransmittingMulticast( "10.0.0.1", "224.0.0.0" ); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.StartTransmittingMulticast( "10.0.0.1", "224.0.0.0" ); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.StartTransmittingMulticast( "10.0.0.1", "224.0.0.0" ); - /// ----- - /// - /// \param ServerIP The IP Address of the server Ethernet interface from which the Multicast data will be sent. - /// Do not specify a port number (any port number specified will be ignored). - /// \param MulticastIP The IP Address of the Multicast group to which Multicast data will be sent. - /// The address must be in the range 224.0.0.0-239.255.255.255. You may also specify - /// the port the data will be sent to by appending it to the IP Address after a colon, - /// e.g. 224.0.0.0:30001. If you do not specify a port it will default to 44801. - /// \return An Output_StartTransmittingMulticast class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + InvalidMulticastIP - /// + ServerAlreadyTransmittingMulticast - Output_StartTransmittingMulticast StartTransmittingMulticast( const String & ServerIP, - const String & MulticastIP ); - /// Ask the DataStream Server to stop transmitting the data you are receiving directly to a Multicast address as well. - /// You must previously have started a transmission via StartTransmittingMulticast. - /// - /// See Also: Connect(), ConnectToMulticast(), Disconnect(), StartTransmittingMulticast() - /// - - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_StartTransmittingMulticast( pClient, "10.0.0.1", "224.0.0.0" ); - /// // Do some stuff - /// Client_StopTransmittingMulticast( pClient ); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.StartTransmittingMulticast( "10.0.0.1", "224.0.0.0" ); - /// // Do some stuff - /// MyClient.StopTransmittingMulticast(); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.StartTransmittingMulticast( "10.0.0.1", "224.0.0.0" ); - /// // Do some stuff - /// MyClient.StopTransmittingMulticast(); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.StartTransmittingMulticast( "10.0.0.1", "224.0.0.0" ); - /// // Do some stuff - /// MyClient.StopTransmittingMulticast(); - /// ----- - /// \return An Output_StopTransmittingMulticast class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + ServerNotTransmittingMulticast - Output_StopTransmittingMulticast StopTransmittingMulticast(); - - /// Enable kinematic segment data in the Vicon DataStream. - /// Call this function on startup, after connecting to the server, and before trying to read local or global segment data. - /// - /// See Also: IsSegmentDataEnabled(), DisableSegmentData(), EnableMarkerData(), EnableUnlabeledMarkerData(), EnableDeviceData(), GetSegmentCount(), GetSegmentName(), GetSegmentGlobalTranslation(), GetSegmentGlobalRotationEulerXYZ(), GetSegmentLocalTranslation(), GetSegmentLocalRotationEulerXYZ() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableSegmentData(); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_EnableSegmentData Output = MyClient.EnableSegmentData(); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Output_EnableSegmentData Output = MyClient.EnableSegmentData(); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Output_EnableSegmentData Output = MyClient.EnableSegmentData(); - /// ----- - /// \return An Output_EnableSegmentData class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - Output_EnableSegmentData EnableSegmentData(); - - /// Enable labeled reconstructed marker data in the Vicon DataStream. - /// Call this function on startup, after connecting to the server, and before trying to read labeled marker data. - /// - /// See Also: IsMarkerDataEnabled(), DisableMarkerData(), EnableSegmentData(), EnableUnlabeledMarkerData(), EnableDeviceData(), GetMarkerCount(), GetMarkerName(), GetMarkerGlobalTranslation() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableMarkerData(); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_EnableMarkerData Output = MyClient.EnableMarkerData(); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Output_EnableMarkerData Output = MyClient.EnableMarkerData(); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Output_EnableMarkerData Output = MyClient.EnableMarkerData(); - /// ----- - /// \return An Output_EnableMarkerData class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - Output_EnableMarkerData EnableMarkerData(); - - /// Enable unlabeled reconstructed marker data in the Vicon DataStream. - /// Call this function on startup, after connecting to the server, and before trying to read global unlabeled marker data. - /// - /// See Also: IsUnlabeledMarkerDataEnabled(), DisableUnlabeledMarkerData(), EnableSegmentData(), EnableMarkerData(), EnableDeviceData(), GetUnlabeledMarkerCount(), GetUnlabeledMarkerGlobalTranslation() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableUnlabeledMarkerData(); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_EnableUnlabeledMarkerData Output = MyClient.EnableUnlabeledMarkerData(); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Output_EnableUnlabeledMarkerData Output = MyClient.EnableUnlabeledMarkerData(); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Output_EnableUnlabeledMarkerData Output = MyClient.EnableUnlabeledMarkerData(); - /// ----- - /// \return An Output_UnlabeledMarkerData class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - Output_EnableUnlabeledMarkerData EnableUnlabeledMarkerData(); - - /// Enable information about the rays contributing to each labeled marker in the Vicon DataStream. - /// Call this function on startup, after connecting to the server, and before trying to read global unlabeled marker data. - /// - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableMarkerRayData(); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_EnableMarkerRayData Output = MyClient.EnableMarkerRayData(); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Output = MyClient.EnableMarkerRayData(); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Output_EnableMarkerRayData Output = MyClient.EnableMarkerRayData(); - /// ----- - /// - /// See Also: IsMarkerRayDataEnabled(), DisableMarkerRayData(), EnableSegmentData(), EnableMarkerData(), EnableDeviceData(), GetUnlabeledMarkerCount(), GetUnlabeledMarkerGlobalTranslation() - /// - /// \return An Output_EnableMarkerRayData class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - Output_EnableMarkerRayData EnableMarkerRayData(); - - /// Enable force plate, EMG, and other device data in the Vicon DataStream. - /// Call this function on startup, after connecting to the server, and before trying to read device information. - /// - /// See Also: IsDeviceDataEnabled(), DisableDeviceData(), EnableSegmentData(), EnableMarkerData(), EnableUnlabeledMarkerData(), GetDeviceCount(), GetDeviceName(), GetDeviceOutputCount(), GetDeviceOutputName(),GetDeviceOutputValue() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableDeviceData(); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_EnableDeviceData Output = MyClient.EnableDeviceData(); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Output = MyClient.EnableDeviceData(); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Output_EnableDeviceData Output = MyClient.EnableDeviceData(); - /// ----- - /// \return An Output_EnableDeviceData class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - Output_EnableDeviceData EnableDeviceData(); - - /// Enable centroid data in the Vicon DataStream. - /// Call this function on startup, after connecting to the server, and before trying to read centroid information. - /// - /// See Also: IsCentroidDataEnabled(), DisableCentroidData() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableCentroidData(); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_EnableCentroidData Output = MyClient.EnableCentroidData(); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Output = MyClient.EnableCentroidData (); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Output_EnableCentroidData Output = MyClient.EnableCentroidData (); - /// ----- - /// \return An Output_EnableCentroidData class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - Output_EnableCentroidData EnableCentroidData(); - - /// Enable greyscale data in the Vicon DataStream. - /// Call this function on startup, after connecting to the server, and before trying to read greyscale information. - /// - /// See Also: IsGreyscaleDataEnabled(), DisableGreyscaleData() - /// - /// - /// C example - /// - /// Not implemented - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_EnableGreyscaleData Output = MyClient.EnableGreyscaleData(); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Output_EnableGreyscaleData Output = MyClient.EnableGreyscaleData (); - /// ----- - /// \return An Output_EnableGreyscaleData class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - Output_EnableGreyscaleData EnableGreyscaleData(); - - /// Enable video data in the Vicon DataStream. - /// Call this function on startup, after connecting to the server, and before trying to read video information. - /// - /// See Also: IsVideoDataEnabled(), DisableVideoData() - /// - /// - /// C example - /// - /// Not implemented - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_EnableVideoData Output = MyClient.EnableVideoData(); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Output_EnableVideoData Output = MyClient.EnableVideoData (); - /// ----- - /// \return An Output_EnableVideoData class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - Output_EnableVideoData EnableVideoData(); - - /// Enable debug data in the Vicon DataStream. - /// Call this function on startup, after connecting to the server in order to receive debug data. - /// - /// See Also: IsDebugDataEnabled(), DisableDebugData() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableDebugData(); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_EnableDebugData Output = MyClient.EnableDebugData(); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Output = MyClient.EnableDebugData (); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Output_EnableDebugData Output = MyClient.EnableDebugData (); - /// ----- - /// \return An Output_EnableDebugData class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - Output_EnableDebugData EnableDebugData(); - - /// Disable kinematic segment data in the Vicon DataStream. - /// - /// See Also: IsSegmentDataEnabled(), EnableSegmentData(), EnableMarkerData(), EnableUnlabeledMarkerData(), EnableDeviceData(), GetSegmentCount(), GetSegmentName(), GetSegmentGlobalTranslation(), GetSegmentGlobalRotationEulerXYZ(), GetSegmentLocalTranslation(), GetSegmentLocalRotationEulerXYZ() - /// - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_DisableSegmentData(); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_DisableSegmentData Output = MyClient.DisableSegmentData(); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Output = MyClient.DisableSegmentData (); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Output_DisableSegmentData Output = MyClient.DisableSegmentData (); - /// ----- - /// \return An Output_DisableSegmentData class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - Output_DisableSegmentData DisableSegmentData(); - - /// Disable labeled reconstructed marker data in the Vicon DataStream. - /// - /// See Also: IsMarkerDataEnabled(), EnableMarkerData(), EnableSegmentData(), EnableUnlabeledMarkerData(), EnableDeviceData(), GetMarkerCount(), GetMarkerName(), GetMarkerGlobalTranslation() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_DisableMarkerData(); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_DisableMarkerData Output = MyClient.DisableMarkerData(); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Output = MyClient.DisableMarkerData (); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Output_DisableMarkerData Output = MyClient.DisableMarkerData (); - /// ----- - /// \return An Output_DisableMarkerData class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - Output_DisableMarkerData DisableMarkerData(); - - /// Disable unlabeled reconstructed marker data in the Vicon DataStream. - /// - /// See Also: IsUnlabeledMarkerDataEnabled(), EnableUnlabeledMarkerData(), EnableSegmentData(), EnableMarkerData(), EnableDeviceData(), GetUnlabeledMarkerCount(), GetUnlabeledMarkerGlobalTranslation() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_DisableUnlabeledMarkerData(); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_DisableUnlabeledMarkerData Output = MyClient.DisableUnlabeledMarkerData(); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Output = MyClient.DisableUnlabeledMarkerData (); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Output_DisableUnlabeledMarkerData Output = MyClient.DisableUnlabeledMarkerData (); - /// ----- - /// \return An Output_DisableUnlabeledMarkerData class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - Output_DisableUnlabeledMarkerData DisableUnlabeledMarkerData(); - - /// Disable unlabeled reconstructed marker data in the Vicon DataStream. - /// - /// See Also: IsMarkerRayDataEnabled(), EnableMarkerRayData(), EnableSegmentData(), EnableMarkerData(), EnableDeviceData(), GetUnlabeledMarkerCount(), GetUnlabeledMarkerGlobalTranslation() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_DisableMarkerRayData(); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_DisableMarkerRayData Output = MyClient.DisableMarkerRayData(); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Output = MyClient.DisableMarkerRayData (); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Output_DisableMarkerRayData Output = MyClient.DisableMarkerRayData (); - /// ----- - /// \return An Output_DisableMarkerRayData class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - Output_DisableMarkerRayData DisableMarkerRayData(); - - /// Disable force plate, EMG, and other device data in the Vicon DataStream. - /// - /// See Also: IsDeviceDataEnabled(), EnableDeviceData(), EnableSegmentData(), EnableMarkerData(), EnableUnlabeledMarkerData(), GetDeviceCount(), GetDeviceName(), GetDeviceOutputCount(), GetDeviceOutputName(), GetDeviceOutputValue() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_DisableDeviceData(); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_DisableDeviceData Output = MyClient.DisableDeviceData(); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Output = MyClient.DisableDeviceData (); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Output_DisableDeviceData Output = MyClient.DisableDeviceData (); - /// ----- - /// \return An Output_DisableDeviceData class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - Output_DisableDeviceData DisableDeviceData(); - - /// Disable centroid data in the Vicon DataStream. - /// - /// See Also: IsCentroidDataEnabled(), EnableCentroidData() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_DisableCentroidData(); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_DisableCentroidData Output = MyClient.DisableCentroidData(); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Output = MyClient.DisableCentroidData (); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Output_DisableCentroidData Output = MyClient.DisableCentroidData (); - /// ----- - /// \return An Output_DisableCentroidData class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - Output_DisableCentroidData DisableCentroidData(); - - /// Disable greyscale data in the Vicon DataStream. - /// - /// See Also: IsGreyscaleDataEnabled(), EnableGreyscaleData() - /// - /// - /// C example - /// - /// Not implemented - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_DisableGreyscaleData Output = MyClient.DisableGreyscaleData(); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Output_DisableGreyscaleData Output = MyClient.DisableGreyscaleData (); - /// ----- - /// - /// \return An Output_DisableGreyscaleData class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - Output_DisableGreyscaleData DisableGreyscaleData(); - - /// Disable video data in the Vicon DataStream. - /// - /// See Also: IsVideoDataEnabled(), EnableVideoData() - /// - /// - /// C example - /// - /// Not implemented - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_DisableVideoData Output = MyClient.DisableVideoData(); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Output_DisableVideoData Output = MyClient.DisableVideoData (); - /// ----- - /// - /// \return An Output_DisableVideoData class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - Output_DisableVideoData DisableVideoData(); - - /// Disable debug data in the Vicon DataStream. - /// - /// See Also: IsDebugDataEnabled(), EnableDebugData() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_DisableDebugData(); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_DisableDebugData Output = MyClient.DisableDebugData(); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Output = MyClient.DisableDebugData (); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Output_DisableDebugData Output = MyClient.DisableDebugData (); - /// ----- - /// - /// \return An Output_DisableDebugData class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - Output_DisableDebugData DisableDebugData(); - - /// Return whether kinematic segment data is enabled in the Vicon DataStream. - /// - /// See Also: EnableSegmentData(), DisableSegmentData(), IsMarkerDataEnabled(), IsUnlabeledMarkerDataEnabled(), IsDeviceDataEnabled() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// CBool Output = Client_IsSegmentDataEnabled( pClient ) - /// // Output == 0 - /// Client_EnabledSegmentData( pClient ); - /// CBool Output = Client_IsSegmentDataEnabled( pClient ) - /// // Output == 1 - /// Client_EnableUnlabeledMarkerData( pClient ); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_IsSegmentDataEnabled Output = MyClient.IsSegmentDataEnabled(); - /// // Output.Enabled == false - /// MyClient.EnableSegmentData(); - /// Output_IsSegmentDataEnabled Output = MyClient.IsSegmentDataEnabled(); - /// // Output.Enabled == true - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Output = MyClient.IsSegmentDataEnabled(); % Output.Enabled == false - /// MyClient.EnableSegmentData(); - /// Output = MyClient.IsSegmentDataEnabled(); % Output.Enabled == true - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Output_IsSegmentDataEnabled Output = MyClient.IsSegmentDataEnabled(); - /// // Output.Enabled == false - /// MyClient.EnableSegmentData(); - /// Output_IsSegmentDataEnabled Output = MyClient.IsSegmentDataEnabled(); - /// // Output.Enabled == true - /// ----- - /// \return An Output_IsSegmentDataEnabled class containing the result of the operation. - /// - The Result will be: - /// + Whether the data is enabled - Output_IsSegmentDataEnabled IsSegmentDataEnabled() const; - - /// Return whether labeled reconstructed marker data is enabled in the DataStream. - /// - /// See Also: EnableMarkerData(), DisableMarkerData(), IsSegmentDataEnabled(), IsUnlabeledMarkerDataEnabled(), IsDeviceDataEnabled() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// CBool Output = Client_IsMarkerDataEnabled( pClient ); - /// // Output = 0; - /// Client_EnableMarkerData( pClient ); - /// CBool Output = Client_IsMarkerDataEnabled( pClient ); - /// // Output = 1; - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_IsMarkerDataEnabled Output = MyClient.IsMarkerDataEnabled(); - /// // Output.Enabled == false - /// MyClient.EnableMarkerData(); - /// Output_IsMarkerDataEnabled Output = MyClient.IsMarkerDataEnabled(); - /// // Output.Enabled == true - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Output = MyClient.IsMarkerDataEnabled(); % Output.Enabled == false - /// MyClient.EnableMarkerData(); - /// Output = MyClient.IsMarkerDataEnabled(); % Output.Enabled == true - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new - /// ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Output_IsMarkerDataEnabled Output = MyClient.IsMarkerDataEnabled(); - /// // Output.Enabled == false - /// MyClient.EnableMarkerData(); - /// Output_IsMarkerDataEnabled Output = MyClient.IsMarkerDataEnabled(); - /// // Output.Enabled == true - /// ----- - /// \return An Output_IsMarkerDataEnabled class containing the result of the operation. - /// - The Result will be: - /// + Whether the data is enabled - Output_IsMarkerDataEnabled IsMarkerDataEnabled() const; - - /// Return whether unlabeled marker data is enabled in the DataStream. - /// - /// See Also: EnableUnlabeledMarkerData(),DisableUnlabeledMarkerData(), IsSegmentDataEnabled(), IsMarkerDataEnabled(), IsDeviceDataEnabled() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// CBool Output = Client_IsUnlabeledMarkerDataEnabled( pClient ); - /// // Output = 0; - /// Client_EnableUnlabeledMarkerData( pClient ); - /// CBool Output = Client_IsUnlabledMarkerDataEnabled( pClient ); - /// // Output = 1; - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_IsUnlabeledMarkerDataEnabled Output = MyClient.IsUnlabeledMarkerDataEnabled(); - /// // Output.Enabled == false - /// MyClient.EnableUnlabeledMarkerData(); - /// Output_IsUnlabeledMarkerDataEnabled Output = MyClient.IsUnlabeledMarkerDataEnabled(); - /// // Output.Enabled == true - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Output = MyClient.IsUnlabeledMarkerDataEnabled(); % Output.Enabled == false - /// MyClient.EnableUnlabeledMarkerData(); - /// Output = MyClient.IsUnlabeledMarkerDataEnabled(); % Output.Enabled == true - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Output_IsUnlabeledMarkerDataEnabled Output = MyClient.IsMarkerDataEnabled(); - /// // Output.Enabled == false - /// MyClient.EnableUnlabeledMarkerData(); - /// Output_IsUnlabeledMarkerDataEnabled Output = MyClient.IsUnlabeledMarkerDataEnabled(); - /// // Output.Enabled == true - /// ----- - /// \return An Output_IsUnlabeledMarkerDataEnabled class containing the result of the operation. - /// - The Result will be: - /// + Whether the data is enabled - Output_IsUnlabeledMarkerDataEnabled IsUnlabeledMarkerDataEnabled() const; - - - /// Return whether marker ray data is enabled in the DataStream. - /// - /// See Also: EnableMarkerRayData(), DisableMarkerRayData(), IsSegmentDataEnabled(), IsMarkerDataEnabled(), IsDeviceDataEnabled() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// CBool Output = Client_IsMarkerRayDataEnabled( pClient ); - /// // Output = 0; - /// Client_EnableMarkerRayData( pClient ); - /// CBool Output = Client_IsMarkerRayDataEnabled( pClient ); - /// // Output = 1; - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_IsMarkerRayDataEnabled Output = MyClient.IsMarkerRayDataEnabled(); - /// // Output.Enabled == false - /// MyClient.EnableMarkerRayData(); - /// Output_IsMarkerRayDataEnabled Output = MyClient.IsMarkerRayDataEnabled(); - /// // Output.Enabled == true - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Output = MyClient.IsMarkerRayDataEnabled(); % Output.Enabled == false - /// MyClient.EnableMarkerRayData(); - /// Output = MyClient.IsMarkerRayDataEnabled(); % Output.Enabled == true - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new - /// ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Output_IsMarkerRayDataEnabled Output = MyClient.IsMarkerDataEnabled(); - /// // Output.Enabled == false - /// MyClient.EnableMarkerRayData(); - /// Output_IsMarkerRayDataEnabled Output = MyClient.IsMarkerRayDataEnabled(); - /// // Output.Enabled == true - /// ----- - /// \return An Output_IsMarkerRayDataEnabled class containing the result of the operation. - /// - The Result will be: - /// + Whether the data is enabled - Output_IsMarkerRayDataEnabled IsMarkerRayDataEnabled() const; - - /// Return whether force plate, EMG, and other device data is enabled in the DataStream. - /// - /// See Also: EnableDeviceData(), DisableDeviceData(), IsSegmentDataEnabled(), IsMarkerDataEnabled(), IsUnlabeledMarkerDataEnabled() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// CBool Output = Client_IsDeviceDataDataEnabled( pClient ); - /// // Output = 0; - /// Client_EnableDeviceDataData( pClient ); - /// CBool Output = Client_IsDeviceDataDataEnabled( pClient ); - /// // Output = 1; - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_IsDeviceDataEnabled Output = MyClient.IsDeviceDataEnabled(); - /// // Output.Enabled == false - /// MyClient.EnableDeviceData(); - /// Output_IsDeviceDataEnabled Output = MyClient.IsDeviceDataEnabled(); - /// Output.Enabled == true - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Output = MyClient.IsDeviceDataEnabled(); % Output.Enabled == false - /// MyClient.EnableDeviceData(); - /// Output = MyClient.IsDeviceDataEnabled(); % Output.Enabled == true - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Output_IsDeviceDataEnabled Output = MyClient.IsDeviceDataEnabled(); - /// // Output.Enabled == false - /// MyClient.EnableDeviceData(); - /// Output_IsDeviceDataEnabled Output = MyClient.IsDeviceDataEnabled(); - /// // Output.Enabled == true - /// ----- - /// \return An Output_IsDeviceDataEnabled class containing the result of the operation. - /// - The Result will be: - /// + Whether the data is enabled - Output_IsDeviceDataEnabled IsDeviceDataEnabled() const; - - /// Return whether Centroid data is enabled in the DataStream. - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// CBool Output = Client_IsCentroidDataDataEnabled( pClient ); - /// // Output = 0; - /// Client_EnableCentroidDataData( pClient ); - /// CBool Output = Client_IsCentroidDataDataEnabled( pClient ); - /// // Output = 1; - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_IsCentroidDataEnabled Output = MyClient.IsCentroidDataEnabled (); - /// // Output.Enabled == false - /// MyClient.EnableCentroidData(); - /// Output_IsCentroidDataEnabled Output = MyClient.IsCentroidDataEnabled (); - /// // Output.Enabled == true - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Output = MyClient.IsCentroidDataEnabled (); % Output.Enabled == false - /// MyClient.EnableCentroidData(); - /// Output = MyClient.IsCentroidDataEnabled (); % Output.Enabled == true - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Output_IsCentroidDataEnabled Output = MyClient.IsCentroidDataEnabled (); - /// // Output.Enabled == false - /// MyClient.EnableCentroidData(); - /// Output_IsCentroidDataEnabled Output = MyClient.IsCentroidDataEnabled (); - /// // Output.Enabled == true - /// ----- - /// - /// See Also: EnableCentroidData(), DisableCentroidData() - /// \return An Output_IsCentroidDataEnabled class containing the result of the operation. - /// - The Result will be: - /// + Whether the data is enabled - Output_IsCentroidDataEnabled IsCentroidDataEnabled() const; - - /// Return whether greyscale data is enabled in the DataStream. - /// - /// See Also: EnableGreyscaleData(), DisableGreyscaleData() - /// - /// - /// C example - /// - /// Not implemented - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_IsGreyscaleDataEnabled Output = MyClient.IsGreyscaleDataEnabled (); - /// // Output.Enabled == false - /// MyClient.EnableGreyscaleData(); - /// Output_IsGreyscaleDataEnabled Output = MyClient.IsGreyscaleDataEnabled (); - /// // Output.Enabled == true - /// - /// MATLAB example - /// - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Output_IsGreyscaleDataEnabled Output = MyClient.IsGreyscaleDataEnabled (); - /// // Output.Enabled == false - /// MyClient.EnableGreyscaleData(); - /// Output_IsGreyscaleDataEnabled Output = MyClient.IsGreyscaleDataEnabled (); - /// // Output.Enabled == true - /// ----- - /// - /// - /// \return An Output_IsGreyscaleDataEnabled class containing the result of the operation. - /// - The Result will be: - /// + Whether the data is enabled - Output_IsGreyscaleDataEnabled IsGreyscaleDataEnabled() const; - - /// Return whether video data is enabled in the DataStream. - /// - /// See Also: EnableVideoData(), DisableVideoData() - /// - /// - /// C example - /// - /// Not implemented - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_IsVideoEnabled Output = MyClient.IsVideoDataEnabled (); - /// // Output.Enabled == false - /// MyClient.EnableVideoData(); - /// Output_IsVideoDataEnabled Output = MyClient.IsVideoDataEnabled (); - /// // Output.Enabled == true - /// - /// MATLAB example - /// - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Output_IsVideoEnabled Output = MyClient.IsVideoDataEnabled (); - /// // Output.Enabled == false - /// MyClient.EnableVideoData(); - /// Output_IsVideoDataEnabled Output = MyClient.IsVideoDataEnabled (); - /// // Output.Enabled == true - /// ----- - /// - /// - /// \return An Output_IsVideoDataEnabled class containing the result of the operation. - /// - The Result will be: - /// + Whether the data is enabled - Output_IsVideoDataEnabled IsVideoDataEnabled() const; - - /// Return whether debug data is enabled in the DataStream. - /// - /// See Also: EnableDebugData(), DisableDebugData() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// CBool Output = Client_IsDebugDataEnabled( pClient ); - /// // Output = 0; - /// Client_EnableDebugData( pClient ); - /// CBool Output = Client_IsDebugDataEnabled( pClient ); - /// // Output = 1; - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_IsDebugDataEnabled Output = MyClient.IsDebugDataEnabled (); - /// // Output.Enabled == false - /// MyClient.EnableDebugData(); - /// Output_IsDebugDataEnabled Output = MyClient.IsDebugDataEnabled (); - /// // Output.Enabled == true - /// - /// MATLAB example - /// - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Output_IsDebugDataEnabled Output = MyClient.IsDebugDataEnabled (); - /// // Output.Enabled == false - /// MyClient.EnableDebugData(); - /// Output_IsDebugDataEnabled Output = MyClient.IsDebugDataEnabled (); - /// // Output.Enabled == true - /// ----- - /// - /// - /// \return An Output_IsDebugDataEnabled class containing the result of the operation. - /// - The Result will be: - /// + Whether the data is enabled - Output_IsDebugDataEnabled IsDebugDataEnabled() const; - - /// Set the number of frames that the client should buffer. - /// The default value is 1, which always supplies the latest frame. - /// Choose higher values to reduce the risk of missing frames between calls. - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_SetBufferSize( 5 ); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.SetBufferSize( 5 ); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.SetBufferSize( 5 ); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.SetBufferSize( 5 ); - /// ----- - /// See Also: GetFrame() - /// - /// \param BufferSize The maximum number of frames to buffer. - /// \return Nothing - void SetBufferSize( unsigned int BufferSize ); - - /// There are three modes that the SDK can operate in. Each mode has a different impact on the Client, Server, and network resources used. - /// - /// + **ServerPush** - /// In "ServerPush" mode, the Server pushes every new frame of data over the network to the Client. - /// The Server will try not to drop any frames. - /// This results in the lowest latency that can be achieved. - /// If the Client is unable to read data at the rate it is being sent, then it is buffered, firstly in the Client, then on the TCP/IP connection, and then at the Server. - /// When all the buffers are full then frames may be dropped at the Server and the performance of the Server may be affected. - /// The GetFrame() method returns the most recently received frame if available, or blocks the calling thread if the most recently received frame has already been processed. - /// - /// + **ClientPull** - /// In "ClientPull" mode, the Client waits for a call to GetFrame(), and then requests the latest frame of data from the Server. - /// This increases latency, because a request must be sent over the network to the Server, the Server has to prepare the frame of data for the Client, and then the data must be sent back over the network. - /// Network bandwidth is kept to a minimum, because the Server only sends what you need. - /// The buffers are very unlikely to be filled, and Server performance is unlikely to be affected. - /// The GetFrame() method blocks the calling thread until the frame has been received. - /// - /// + **ClientPullPreFetch** - /// "ClientPullPreFetch" is an enhancement to the "ClientPull" mode. - /// A thread in the SDK continuously and preemptively does a "ClientPull" on your behalf, storing the latest requested frame in memory. - /// When you next call GetFrame(), the SDK returns the last requested frame that was cached in memory. - /// GetFrame() does not need to block the calling thread. - /// As with normal "ClientPull", buffers are unlikely to fill up, and Server performance is unlikely to be affected. - /// Latency is slightly reduced, but network traffic may increase if you request frames on behalf of the Client which are never used. - // - /// The stream defaults to "ClientPull" mode as this is considered the safest option. If performance is a problem, try "ClientPullPreFetch" followed by "ServerPush". - /// - /// See Also: GetFrame(), GetLatencyTotal() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_SetStreamMode( pClient, ServerPush ); - /// Client_SetStreamMode( pClient, ClientPull ); - /// Client_SetStreamMode( pClient, ClientPullPreFetch ); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ServerPush ); - /// MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ClientPull ); - /// MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ClientPullPreFetch ); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( 'localhost' ); - /// MyClient.SetStreamMode( StreamMode.ServerPush ); - /// MyClient.SetStreamMode( StreamMode.ClientPull ); - /// MyClient.SetStreamMode( StreamMode.ClientPullPreFetch ); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.SetStreamMode( ViconDataStreamSDK.DotNET.StreamMode.ServerPush ); - /// MyClient.SetStreamMode( ViconDataStreamSDK.DotNET.StreamMode.ClientPull ); - /// MyClient.SetStreamMode( ViconDataStreamSDK.DotNET.StreamMode.ClientPullPreFetch); - /// ----- - /// \param Mode Stream modes that the SDK can operate in - /// + StreamMode.ServerPush - /// + StreamMode.ClientPull - /// + StreamMode.ClientPullPreFetch - /// - /// \return An Output_SetStreamMode class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - Output_SetStreamMode SetStreamMode( const StreamMode::Enum Mode ); - - /// Enable haptic feedback for the selected Apex device. - /// - /// Apex device names may be obtained using GetDeviceCount, GetDeviceName - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// Client_SetApexDeviceFeedback( pClient, "ViconApex_01", true ); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame() - /// Output_GetDeviceName DeviceName MyClient.GetDeviceName( 0 ); - /// MyClient.SetApexDeviceFeedback( DeviceName.DeviceName, true ); - /// - /// MATLAB example - /// - /// - /// .NET example - /// - /// ----- - /// - /// \return An Output_SetApexDeviceFeedback class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidDeviceName - /// + NullClient - /// + HapticAlreadySet - Output_SetApexDeviceFeedback SetApexDeviceFeedback( const String& DeviceName, bool i_bOn ); - - /// Remaps the 3D axis. - /// Vicon Data uses a right-handed coordinate system, with +X forward, +Y left, and +Z up. - /// Other systems use different coordinate systems. The SDK can transform its data into any valid right-handed coordinate system by re-mapping each axis. - /// Valid directions are "Up", "Down", "Left", "Right", "Forward", and "Backward". Note that "Forward" means moving away from you, and "Backward" is moving towards you. - /// Common usages are - /// Z-up: SetAxisMapping( Forward, Left, Up ) - /// Y-up: SetAxisMapping( Forward, Up, Right ) - /// - /// See Also: GetAxisMapping() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_SetAxisMapping(pClient, Forward, Left, Up); // Z-up - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.SetAxisMapping( ViconDataStreamSDK::CPP::Direction::Forward, - /// ViconDataStreamSDK::CPP::Direction::Left, - /// ViconDataStreamSDK::CPP::Direction::Up ); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.SetAxisMapping( Direction.Forward, Direction.Left, Direction.Up ); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.SetAxisMapping( ViconDataStreamSDK.DotNET.Direction.Forward, - /// ViconDataStreamSDK.DotNET.Direction.Left, - /// ViconDataStreamSDK.DotNET.Direction.Up ); - /// ----- - /// \param XAxis Specify the direction of your X axis relative to yourself as the observer. - /// \param YAxis Specify the direction of your Y axis relative to yourself as the observer. - /// \param ZAxis Specify the direction of your Z axis relative to yourself as the observer. - /// - /// \return An Output_SetAxisMapping class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + CoLinearAxes - /// + LeftHandedAxes - Output_SetAxisMapping SetAxisMapping( const Direction::Enum XAxis, const Direction::Enum YAxis, const Direction::Enum ZAxis ); - - /// Get the current Axis mapping. - /// - /// See Also: SetAxisMapping() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// COutput_GetAxisMapping _Output_GetAxisMapping; - /// Client_GetAxisMapping( pClient, &_Output_GetAxisMapping ); - /// // _Output_GetAxisMapping.XAxis == Forward - /// // _Output_GetAxisMapping.YAxis == Left - /// // _Output_GetAxisMapping.ZAxis == Up - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// Output_GetAxisMapping Output = MyClient.GetAxisMapping(); - /// // Output.XAxis == ViconDataStreamSDK::CPP::Direction::Forward - /// // Output.YAxis == ViconDataStreamSDK::CPP::Direction::Left - /// // Output.ZAxis == ViconDataStreamSDK::CPP::Direction::Up - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// Output = MyClient.GetAxisMapping(); - /// % Output.XAxis == Direction.Forward - /// % Output.YAxis == Direction.Left - /// % Output.ZAxis == Direction.Up - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// Output_GetAxisMapping Output = MyClient.GetAxisMapping(); - /// // Output.XAxis == ViconDataStreamSDK.DotNET.Direction.Forward - /// // Output.YAxis == ViconDataStreamSDK.DotNET.Direction.Left - /// // Output.ZAxis == ViconDataStreamSDK.DotNET.Direction.Up - /// ----- - /// \return An Output_GetAxisMapping class containing the result of the operation. - /// - The Result will be: - /// + XAxis, YAxis, ZAxis - Output_GetAxisMapping GetAxisMapping() const; - - /// @private - Output_GetServerOrientation GetServerOrientation() const; - - /// Request a new frame to be fetched from the Vicon DataStream Server. - /// - /// See Also: SetStreamMode() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// CEnum Output = Client_GetFrame( pClient ); // Output == NotConnected - /// Client_Connect( pClient, "localhost" ); - /// Output = Client_GetFrame( pClient ); // Output == Success - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// Output_GetFrame Output; - /// Output = MyClient.GetFrame(); // Output.Result == NotConnected - /// MyClient.Connect( "localhost" ); - /// Output = MyClient.GetFrame(); // Output.Result == Success - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// Output = MyClient.GetFrame(); // Output.Result == NotConnected - /// MyClient.Connect( "localhost" ); - /// Output = MyClient.GetFrame(); // Output.Result == Success - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// Output_GetFrame Output; - /// Output = MyClient.GetFrame(); // Output.Result == NotConnected - /// MyClient.Connect( "localhost" ); - /// Output = MyClient.GetFrame(); // Output.Result == Success - /// ----- - /// \return An Output_GetFrame class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - Output_GetFrame GetFrame(); - - /// Return the number of the last frame retrieved from the DataStream. - /// - /// See Also: GetFrame(), GetTimecode() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// COutput_GetFrameNumber _Output_GetFrameNumber; - /// Client_GetFrameNumber(pClient, &_Output_GetFrameNumber); // _Output_GetFrameNumber.FrameNumber == 0; - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrameNumber(pClient, &_Output_GetFrameNumber); // _Output_GetFrameNumber.FrameNumber > 1; - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_GetFrameNumber Output; - /// Output = MyClient.GetFrameNumber(); // Output.Result == NoFrame - /// // Output.FrameNumber == 0 - /// MyClient.GetFrame(); - /// Output = MyClient.GetFrameNumber(); // Output.Result == Success - /// // Output.FrameNumber >= 1 - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Output = MyClient.GetFrameNumber(); % Output.Result == NoFrame - /// % Output.FrameNumber == 0 - /// MyClient.GetFrame(); - /// Output = MyClient.GetFrameNumber(); % Output.Result == Success - /// % Output.FrameNumber >= 1 - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Output_GetFrameNumber Output; - /// Output = MyClient.GetFrameNumber(); // Output.Result == NoFrame - /// // Output.FrameNumber == 0 - /// MyClient.GetFrame(); - /// Output = MyClient.GetFrameNumber(); // Output.Result == Success - /// // Output.FrameNumber >= 1 - /// ----- - /// - /// - /// - /// \return An Output_GetFrameNumber class containing the result of the operation and the frame number. - /// - The Result will be: - /// + Success - /// + NotConnected - Output_GetFrameNumber GetFrameNumber() const; - - /// Return the timecode information for the last frame retrieved from the DataStream. If the stream is valid but timecode is not available, the Output will be Result.Success and the Standard will be None. - /// - /// See Also: GetFrame(), GetFrameNumber() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// COutput_GetTimecode _Output_Timecode; - /// Client_GetTimecode( pClient, &_Output_Timecode ); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetTimecode Output = MyClient.GetTimecode(); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetTimecode(); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetTimecode Output = MyClient.GetTimecode(); - /// ----- - /// \return An Output_GetTimecode class containing the result of the operation and - /// + Hours - /// + Minutes - /// + Seconds - /// + Frames - /// + SubFrame - /// + FieldFlag - /// + Standard - /// + SubFramesPerFrame - /// + UserBits - /// - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - Output_GetTimecode GetTimecode() const; - - /// Return the Vicon camera system frame rate (in Hz) at the time of the last frame retrieved from the DataStream. - /// - /// See Also: GetFrame(), GetFrameNumber(), GetTimecode() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// COutput_GetFrameRate Rate; - /// Client_GetFrameRate(pClient, &Rate); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetFrameRate Output = MyClient.GetFrameRate (); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetFrameRate (); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetFrameRate Output = MyClient.GetFrameRate (); - /// ----- - /// \return An Output_GetFrameRate class containing the result of the operation and the frame rate in hz. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - Output_GetFrameRate GetFrameRate() const; - - /// Return the number of latency measurements that were taken at various stages of the real-time pipeline. This value can be passed into GetLatencySampleName(). - /// - /// See Also: GetFrame(), GetTimecode(), GetLatencyTotal(), GetLatencySampleName(), GetLatencySampleValue() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// COutput_GetLatencySampleCount _Output_LatencySampleCount; - /// Client_GetLatencySampleCount( pClient, &_Output_LatencySampleCount ); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetLatencySampleCount Output = MyClient.GetLatencySampleCount(); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( 'localhost' ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetLatencySampleCount(); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetLatencySampleCount Output = MyClient.GetLatencySampleCount(); - /// ----- - /// \return An Output_GetLatencySampleCount class containing the result of the operation and the number of samples taken. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - Output_GetLatencySampleCount GetLatencySampleCount() const; - - /// Return the name of a latency sample. This value can be passed into GetLatencySampleValue(). - /// - /// See Also: GetFrame(), GetTimecode(), GetLatencyTotal(), GetLatencySampleCount(), GetLatencySampleValue() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// char SampleName[128]; - /// Client_GetLatencySampleName(pClient, 0, 128, SampleName); - /// // SampleName = "Data Collected" - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetLatencySampleName Output = MyClient.GetLatencySampleName( 0 ); - /// // Output.Name == "Data Collected" - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( 'localhost' ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetLatencySampleName( 1 ); - /// % Output.Name == 'Data Collected' - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetLatencySampleName Output = MyClient.GetLatencySampleName( 0 ); - /// Output.Name == "Data Collected" - /// ----- - /// \param LatencySampleIndex The index of the name. - /// \return An Output_GetLatencySampleName class containing the result of the operation and the name of the latency sample. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - Output_GetLatencySampleName GetLatencySampleName( const unsigned int LatencySampleIndex ) const; - - /// Return the duration of a named latency sample in seconds. This value can be passed into GetLatencySampleValue(). - /// - /// See Also: GetFrame(), GetTimecode(), GetLatencyTotal(), GetLatencySampleCount(), GetLatencySampleValue() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// COutput_GetLatencySampleValue _Output_GetLatencySampleValue ; - /// Client_GetLatencySampleValue ( pClient, "Data Collected", &_Output_GetLatencySampleValue ); - /// // _Output_GetLatencySampleValue.Value = 0.1 - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetLatencySampleValue Output = - /// MyClient.GetLatencySampleValue( "Data Collected" ); - /// // Output.Value == 0.1 - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( 'localhost' ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetLatencySampleValue( 'Data Collected' ); - /// % Output.Value == 0.1 - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetLatencySampleName Output = - /// MyClient.GetLatencySampleValue( "Data Collected" ); - /// // Output.Value == 0.1 - /// ----- - /// \param LatencySampleName The name of the latency sample - /// \return An Output_GetLatencySampleValue class containing the result of the operation and the duration of the latency in seconds. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - Output_GetLatencySampleValue GetLatencySampleValue( const String & LatencySampleName ) const; - - /// Return the total latency in seconds introduced at various stages of the real-time pipeline. - /// If no latency information is available then all latencies will be reported as 0.0. - /// - /// See Also: GetFrame(), GetTimecode(), GetLatencySampleCount(), GetLatencySampleName(), GetLatencySampleValue() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// COutput_GetLatencyTotal _Output_GetLatencyTotal ; - /// Client_GetLatencyTotal ( pClient, &_Output_GetLatencyTotal ); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetLatencyTotal Output = MyClient.GetLatencyTotal(); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( 'localhost' ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetLatencyTotal(); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetLatencyTotal Output = MyClient.GetLatencyTotal(); - /// ----- - /// \return An Output_GetLatencyTotal class containing the result of the operation and the total latency in seconds made from summing the other latencies. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - Output_GetLatencyTotal GetLatencyTotal() const; - - /// Returns the hardware frame number as used by the cameras. This is not reset on synchronization. - /// - /// See Also: GetFrameNumber() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// COutput_GetHardwareFrameNumber _Output_GetHardwareFrameNumber ; - /// Client_GetHardwareFrameNumber ( pClient, &_Output_GetHardwareFrameNumber ); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetHardwareFrameNumber Output = MyClient.GetHardwareFrameNumber(); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetHardwareFrameNumber Output = MyClient.GetHardwareFrameNumber(); - /// ----- - /// - /// \return An Output_GetHardwareFrameNumber class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - Output_GetHardwareFrameNumber GetHardwareFrameNumber() const; - - /// Get the number of frame rate types that the server application reports. - /// - /// See Also: GetFrameRateName(), GetFrameRateValue() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// COutput_GetFrameRateCount _Output_GetFrameRateCount; - /// Client_GetFrameRateCount ( pClient, &_Output_GetFrameRateCount ); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetFrameRateCount Output = MyClient.GetFrameRateCount(); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( 'localhost' ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetFrameRateCount(); - /// % Output.Count = 3 - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetFrameRateCount Output = MyClient.GetFrameRateCount(); - /// ----- - /// - /// \return An Output_GetFrameRateCount class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - Output_GetFrameRateCount GetFrameRateCount() const; - - /// Get the name of a frame rate type at the specified index. - /// - /// See Also: GetFrameRateCount(), GetFrameRateValue() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// char FramerateName[128]; - /// Client_GetFrameRateName(pClient, 0, 128, FramerateName); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// A valid index is between 0 and GetFrameRateCount() - 1 - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetFrameRateCount Output = MyClient.GetFrameRateCount(); - /// if( Output.Count > 0 ) - /// { - /// Output_GetFrameRateName NameOutput = MyClient.GetFrameRateIndex( 0 ); - /// } - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( 'localhost' ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetFrameRateName( 1 ); - /// % Output.Name = 'name' - /// - /// .NET example - /// - /// A valid index is between 0 and GetFrameRateCount() - 1 - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetFrameRateCount Output = MyClient.GetFrameRateCount(); - /// if( Output.Count > 0 ) - /// { - /// Output_GetFrameRateName NameOutput = MyClient.GetFrameRateIndex( 0 ); - /// } - /// ----- - /// - /// \return An Output_GetFrameRateName class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + InvalidIndex - Output_GetFrameRateName GetFrameRateName( const unsigned int FrameRateIndex ) const; - - /// Get the current value of the specified frame rate type. - /// - /// See Also: GetFrameRateCount(), GetFrameRateName() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// char FramerateName[128]; - /// Client_GetFrameRateName(pClient, 0, 128, FramerateName); - /// COutput_GetFrameRateValue FramerateValue; - /// Client_GetFrameRateValue(pClient, FramerateName, &FramerateValue); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// A valid name is obtained from GetFrameRateName - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetFrameRateCount Output = MyClient.GetFrameRateCount(); - /// Output_GetFrameRateName NameOutput = MyClient.GetFrameRateIndex( 0 ); - /// Output_GetFrameRateValue ValueOutput = MyClient.GetFrameRateValue( NameOutput.Name ); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( 'localhost' ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetFrameRateName( 1 ); - /// ValueOutput = MyClient.GetFrameRateValue( Output.Name ); - /// % Output.Value = '200' - /// - /// .NET example - /// - /// A valid name is obtained from GetFrameRateName - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetFrameRateCount Output = MyClient.GetFrameRateCount(); - /// Output_GetFrameRateName NameOutput = MyClient.GetFrameRateIndex( 0 ); - /// Output_GetFrameRateValue ValueOutput = MyClient.GetFrameRateValue( NameOutput.Name ); - /// ----- - /// - /// \return An Output_GetFrameRateValue class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + InvalidFrameRateName - Output_GetFrameRateValue GetFrameRateValue( const String & FrameRateName ) const; - - /// Return the number of subjects in the DataStream. This information can be used in conjunction with GetSubjectName. - /// - /// See Also: GetSubjectName() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// COutput_GetSubjectCount SubjectCount; - /// Client_GetSubjectCount(pClient, &SubjectCount); // SubjectCount.Result == NoFrame - /// // SubjectCount.SubjectCount == 0; - /// Client_GetFrame( pClient ); - /// Client_GetSubjectCount(pClient, &SubjectCount); // SubjectCount.Result == Success; - /// // SubjectCount.SubjectCount == 0; - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_GetSubjectCount Output; - /// Output = MyClient.GetSubjectCount(); // Output.Result == NoFrame - /// // Output.SubjectCount == 0 - /// MyClient.GetFrame(); - /// Output = MyClient.GetSubjectCount(); // Output.Result == Success - /// // Output.SubjectCount >= 0 - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( 'localhost' ); - /// Output = MyClient.GetSubjectCount(); % Output.Result == NoFrame - /// % Output.SubjectCount == 0 - /// MyClient.GetFrame(); - /// Output = MyClient.GetSubjectCount(); % Output.Result == Success - /// % Output.SubjectCount >= 0 - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Output_GetSubjectCount Output; - /// Output = MyClient.GetSubjectCount(); // Output.Result == NoFrame - /// // Output.SubjectCount == 0 - /// MyClient.GetFrame(); - /// Output = MyClient.GetSubjectCount(); // Output.Result == Success - /// // Output.SubjectCount >= 0 - /// ----- - /// - /// - /// - /// \return An Output_GetSubjectCount class containing the result of the operation and the number of subjects. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - Output_GetSubjectCount GetSubjectCount() const; - - /// Return the name of a subject. This can be passed into segment and marker functions. - /// - /// See Also: GetSubjectCount() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// char SubjectName[128]; - /// CEnum Output = Client_GetSubjectName(pClient, 0, 128, SubjectName); - /// // Output == Success - /// // SubjectName =="AI" - /// Output = Client_GetSubjectName(pClient, 1, 128, SubjectName); - /// // Output == Success - /// // SubjectName =="Bob" - /// Output = Client_GetSubjectName(pClient, 2, 128, SubjectName); - /// // Output == InvalidIndex - /// // SubjectName =="" - /// - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSubjectCount OutputGSC; - /// OutputGSC = MyClient.GetSubjectCount(); // OutputGSC.Result == Success - /// // OutputGSC.SubjectCount == 2 - /// Output_GetSubjectName OutputGSN; - /// OutputGSN = MyClient.GetSubjectName(0);// OutputGSN.Result == Success - /// // OutputGSN.SubjectName == "Al" - /// OutputGSN = MyClient.GetSubjectName(1);// OutputGSN.Result == Success - /// // OutputGSN .SubjectName == "Bob" - /// OutputGSN = MyClient.GetSubjectName(2);// OutputGSN.Result == InvalidIndex - /// // OutputGSN.SubjectName == "" - /// - /// MATLAB example - /// - /// MyClient = Client; - /// MyClient.Connect( 'localhost' ); - /// MyClient.GetFrame(); - /// OutputGSC = MyClient.GetSubjectCount(); % OutputGSC.Result == Success - /// % OutputGSC.SubjectCount == 2 - /// OutputGSN = MyClient.GetSubjectName(1); % OutputGSN.Result == Success - /// % OutputGSN.SubjectName == 'Al' - /// OutputGSN = MyClient.GetSubjectName(2); % OutputGSN.Result == Success - /// % OutputGSN .SubjectName == 'Bob' - /// OutputGSN = MyClient.GetSubjectName(3); % OutputGSN.Result == InvalidIndex - /// % OutputGSN.SubjectName == '' - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = - /// new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSubjectCount OutputGSC; - /// OutputGSC = MyClient.GetSubjectCount(); // OutputGSC.Result == Success - /// // OutputGSC.SubjectCount == 2 - /// Output_GetSubjectName OutputGSN; - /// OutputGSN = MyClient.GetSubjectName(0);// OutputGSN.Result == Success - /// // OutputGSN.SubjectName == "Al" - /// OutputGSN = MyClient.GetSubjectName(1);// OutputGSN.Result == Success - /// // OutputGSN .SubjectName == "Bob" - /// OutputGSN = MyClient.GetSubjectName(2);// OutputGSN.Result == InvalidIndex - /// // OutputGSN.SubjectName == "" - /// ----- - /// \param SubjectIndex The index of the subject. A valid Subject Index is between 0 and GetSubjectCount()-1. - /// Matlab: A valid Subject Index is between 1 and GetSubjectCount(). - /// \return An Output_GetSubjectName GetSubjectName class containing the result of the operation and the name of the subject. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - Output_GetSubjectName GetSubjectName( const unsigned int SubjectIndex ) const; - - /// Return the name of the root segment for a specified subject. This can be passed into segment functions.The root segment is the ancestor of all other segments in the subject. - /// - /// See Also: GetSegmentCount(), GetSegmentParentName(), GetSegmentChildCount(), GetSegmentChildName() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableSegmentData( pClient ); - /// Client_GetFrame( pClient ); - /// char RootSegment[128]; - /// CEnum Result = Client_GetSubjectRootSegmentName(pClient, "Bob", 128, RootSegment); - /// // Result == Success - /// // RootSegment == "Pelvis" - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSubjectRootSegmentName Output; - /// Output = MyClient.GetSubjectRootSegmentName( "Bob" ); - /// // Output.Result == Success - /// // Output.SegmentName == "Pelvis" - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSubjectRootSegmentName( "Bob" ); - /// % Output.Result == Success - /// % Output.SegmentName == "Pelvis" - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSubjectRootSegmentName Output; - /// Output = MyClient.GetSubjectRootSegmentName( "Bob" ); - /// // Output.Result == Success - /// // Output.SegmentName == "Pelvis" - /// ----- - /// \param SubjectName The name of the subject - /// \return An Output_GetSubjectRootSegmentName class containing the result of the operation and the name of the root segment. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - Output_GetSubjectRootSegmentName GetSubjectRootSegmentName( const String & SubjectName ) const; - - /// Return the number of segments for a specified subject in the DataStream. This information can be used in conjunction with GetSegmentName. - /// - /// See Also: GetSubjectName(), GetSegmentName() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// COutput_GetSegmentCount SegmentCount; - /// Client_GetSegmentCount( pClient, "Bob", &SegmentCount ); - /// // SegmentCount.Result == NOFrame - /// // SegmentCount.Value == 0 - /// Client_GetFrame( pClient ); - /// Client_GetSegmentCount( pClient, "AI", &SegmentCount ); - /// // SegmentCount.Result == InvalidSubjectName - /// // SegmentCount.Value == 0 - /// Client_GetSegmentCount( pClient, "Bob", &SegmentCount ); - /// // SegmentCount.Result == Success - /// // SegmentCount.Value >= 0 - /// - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.EnableSegmentData(); - /// MyClient.Connect( "localhost" ); - /// Output_GetSegmentCount Output; - /// Output = MyClient.GetSegmentCount( "Bob" ); // Output.Result == NoFrame - /// // Output.SegmentCount == 0 - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentCount( "Al" ); // Output.Result == - /// // InvalidSubjectName - /// // Output.SegmentCount == 0 - /// Output = MyClient.GetSegmentCount( "Bob" );// Output.Result == Success - /// // Output.SegmentCount >= 0 - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.EnableSegmentData(); - /// MyClient.Connect( "localhost" ); - /// Output = MyClient.GetSegmentCount( "Bob" ); % Output.Result == NoFrame - /// % Output.SegmentCount == 0 - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentCount( "Al" ); % Output.Result == - /// % InvalidSubjectName - /// % Output.SegmentCount == 0 - /// Output = MyClient.GetSegmentCount( "Bob" ); % Output.Result == Success - /// % Output.SegmentCount >= 0 - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.EnableSegmentData(); - /// MyClient.Connect( "localhost" ); - /// Output_GetSegmentCount Output; - /// Output = MyClient.GetSegmentCount( "Bob" ); // Output.Result == NoFrame - /// // Output.SegmentCount == 0 - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentCount( "Al" ); // Output.Result == - /// // InvalidSubjectName - /// // Output.SegmentCount == 0 - /// Output = MyClient.GetSegmentCount( "Bob" ); // Output.Result == Success - /// // Output.SegmentCount >= 0 - /// ----- - /// - /// - /// - /// \param SubjectName The name of the subject. - /// \return An Output_GetSegmentCount class containing the result of the operation and the number of segments. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - Output_GetSegmentCount GetSegmentCount( const String & SubjectName ) const; - - /// Return the name of a subject segment specified by index. - /// - /// See Also: GetSegmentCount(), GetSegmentChildCount(), GetSegmentChildName(), GetSubjectRootSegmentName() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// char SegmentName[128]; - /// // SegmentIndex must be between 0 and GetSegmentCount() - 1 - /// Client_GetSegmentName(pClient, "Bob", 0, 128, SegmentName); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentName Output; - /// // SegmentIndex must be between 0 and GetSegmentCount() - 1 - /// Output = MyClient.GetSegmentName( "Bob", 0 ); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// % SegmentIndex must be between 1 and GetSegmentCount() - /// Output = MyClient.GetSegmentName( "Bob", 1 ); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentParentName Output; - /// // SegmentIndex must be between 0 and GetSegmentCount() - 1 - /// Output = MyClient.GetSegmentName( "Bob", 0 ); - /// ----- - /// \param SubjectName The name of the subject - /// \param SegmentIndex The index of the segment - /// \return An Output_GetSegmentName class containing the result of the operation and the name of the parent segment or an empty string if it is the root segment. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - /// + InvalidSubjectName - Output_GetSegmentName GetSegmentName( const String & SubjectName, - const unsigned int SegmentIndex ) const; - /// Return the number of child segment for a specified subject segment. This can be passed into segment functions. - /// - /// See Also: GetSegmentCount() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// COutput_GetSegmentChildCount ChildCount; - /// Client_GetSegmentChildCount(pClient, "Bob", "Pelvis", &ChildCount); - /// // ChildCount.Result == Success - /// // ChildCount.SegmentCount == 2 - /// Client_GetSegmentChildCount(pClient, "Alice", "Pelvis", &ChildCount); - /// // ChildCount.Result == InvalidSubjectName - /// // ChildCount.SegmentCount == 0 - /// - /// char SegmentName[128]; - /// Client_GetSegmentName(pClient, "Bob", , 128, SegmentName); - /// Client_GetSegmentName(pClient, "Bob", &SegmentName); - /// // ChildCount.Result == Success - /// // ChildCount.SegmentCount == 2 - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentChildCount OutputGSCC; - /// OutputGSCC = MyClient.GetSegmentChildCount( "Bob", "Pelvis" ); - /// // OutputGSCC.Result == Success - /// // OutputGSCC.SegmentCount == 2 - /// Output_GetSegmentChildName OutputGSCN; - /// OutputGSCN = MyClient.GetSegmentName( "Alice", 0 ); - /// // OutputGSN.Result == InvalidSubjectName - /// // OutputGSN.SegmentName == "" - /// OutputGSCN = MyClient.GetSegmentName( "Bob", "Pelvis", 0 ); - /// // OutputGSCN.Result == Success - /// // OutputGSCN.SegmentName == "LFemur" - /// OutputGSCN = MyClient.GetSegmentName( "Bob", "Pelvis", 1 ); - /// // OutputGSCN.Result == Success - /// // OutputGSCN.SegmentName == "RFemur" - /// OutputGSCN = MyClient.GetSegmentName( "Bob", "Pelvis", 2 ); - /// // OutputGSCN.Result == InvalidIndex - /// // OutputGSCN.SegmentName == "" - /// // (no third segment) - /// - /// MATLAB example - /// - /// A valid Segment Index is between 1 and GetSegmentChildCount() - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// OutputGSCC = MyClient.GetSegmentChildCount( "Bob", "Pelvis" ); - /// % OutputGSCC.Result == Success - /// % OutputGSCC.SegmentCount == 2 - /// OutputGSCN = MyClient.GetSegmentChildName( "Alice", "Pelvis", 1 ); - /// % OutputGSCN.Result == InvalidSubjectName - /// % OutputGSCN.SegmentName == "" - /// OutputGSCN = MyClient.GetSegmentChildName( "Bob", "Pelvis", 1 ); - /// % OutputGSCN.Result == Success - /// % OutputGSCN.SegmentName == "LFemur" - /// OutputGSCN = MyClient.GetSegmentChildName( "Bob", "Pelvis", 2 ); - /// % OutputGSCN.Result == Success - /// % OutputGSCN.SegmentName == "RFemur" - /// OutputGSCN = MyClient.GetSegmentChildName( "Bob", "Pelvis", 3 ); - /// % OutputGSCN.Result == InvalidIndex - /// % OutputGSCN.SegmentName == "" - /// % (no third segment) - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentChildCount OutputGSCC; - /// OutputGSCC = MyClient.GetSegmentChildCount( "Bob", "Pelvis" ); - /// // OutputGSCC.Result == Success - /// // OutputGSCC.SegmentCount == 2 - /// Output_GetSegmentChildName OutputGSCN; - /// OutputGSCN = MyClient.GetSegmentChildName( "Alice", "Pelvis", 0 ); - /// // OutputGSCN.Result == InvalidSubjectName - /// // OutputGSCN.SegmentName == "" - /// OutputGSCN = MyClient.GetSegmentChildName( "Bob", "Pelvis", 0 ); - /// // OutputGSCN.Result == Success - /// // OutputGSCN.SegmentName == "LFemur" - /// OutputGSCN = MyClient.GetSegmentChildName( "Bob", "Pelvis", 1 ); - /// // OutputGSCN.Result == Success - /// // OutputGSCN.SegmentName == "RFemur" - /// OutputGSCN = MyClient.GetSegmentChildName( "Bob", "Pelvis", 2 ); - /// // OutputGSCN.Result == InvalidIndex - /// // OutputGSCN.SegmentName == "" - /// // (no third segment) - /// ----- - /// \param SubjectName The name of the subject - /// \param SegmentName The name of the segment - /// \return An Output_GetSegmentChildCount class containing the result of the operation and the number of child segments. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - /// + InvalidSubjectName - /// + InvalidSegmentName - Output_GetSegmentChildCount GetSegmentChildCount( const String & SubjectName, - const String & SegmentName ) const; - - /// Return the name of the child segment for a specified subject segment and index. - /// - /// See Also: GetSegmentCount(), GetSegmentChildCount(), GetSegmentChildName(), GetSubjectRootSegmentName() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableSegmentData( pClient ); - /// Client_GetFrame( pClient ); - /// char SegmentChildName[128]; - /// // Segment index must be between 0 and Client_GetSegmentChildCount() - 1 - /// Client_GetSegmentChildName( pClient, "Bob", "Pelvis", 0, 128, SegmentChildName ); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentChildName Output; - /// // Segment index must be between 0 and GetSegmentChildCount() - 1 - /// Output = MyClient.GetSegmentChildName( "Bob", "Pelvis", 0 ); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// // Segment index must be between 1 and GetSegmentChildCount() - /// Output = MyClient.GetSegmentChildName( "Bob", "Pelvis", 1 ); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentChildName Output; - /// // Segment index must be between 0 and GetSegmentChildCount() - 1 - /// Output = MyClient.GetSegmentChildName( "Bob", "Pelvis", 0 ); - /// ----- - /// \param SubjectName The name of the subject - /// \param SegmentName The name of the segment - /// \param SegmentIndex The index of the child segment. A valid Segment Index is between 0 and GetSegmentChildCount()-1. - /// \return An Output_GetSegmentChildName class containing the result of the operation and the name of the child segment. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - /// + InvalidSubjectName - /// + InvalidSegmentName - Output_GetSegmentChildName GetSegmentChildName( const String & SubjectName, - const String & SegmentName, - const unsigned int SegmentIndex ) const; - - /// Return the name of the parent segment for a specified subject segment. If the specified segment is the root segment of the subject then it will return an empty string. - /// - /// See Also: GetSegmentCount(), GetSegmentChildCount(), GetSegmentChildName(), GetSubjectRootSegmentName() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// char SegmentParentName[128]; - /// CEnum Result = Client_GetSegmentParentName(pClient, "Bob", "Pelvis", 128, SegmentParentName); - /// // Result == Success - /// // SegmentParentName = "" - /// // This is the root segment - /// Result = Client_GetSegmentParentName(pClient, "Bob", "LFemur", 128, SegmentParentName); - /// // Result == Success - /// // SegmentParentName = "Pelvis" - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentParentName Output; - /// Output = MyClient.GetSegmentParentName( "Bob", "Pelvis" ); - /// // Output.Result == Success - /// // Output.SegmentName == "" - /// // This is the root segment - /// Output = MyClient.GetSegmentParentName( "Bob", "LFemur" ); - /// // Output.Result == Success - /// // Output.SegmentName == "Pelvis" - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentParentName( "Bob", "Pelvis" ); - /// % Output.Result == Success - /// % Output.SegmentCount == "" - /// % This is the root segment - /// Output = MyClient.GetSegmentParentName( "Bob", "LFemur" ); - /// % Output.Result == Success - /// % Output.SegmentCount == "Pelvis" - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentParentName Output; - /// Output = MyClient.GetSegmentParentName( "Bob", "Pelvis" ); - /// // Output.Result == Success - /// // Output.SegmentName == "" - /// // This is the root segment - /// Output = MyClient.GetSegmentParentName( "Bob", "LFemur" ); - /// // Output.Result == Success - /// // Output.SegmentName == "Pelvis" - /// ----- - /// \param SubjectName The name of the subject - /// \param SegmentName The name of the segment - /// \return An Output_GetSegmentParentName class containing the result of the operation and the name of the parent segment or an empty string if it is the root segment. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - Output_GetSegmentParentName GetSegmentParentName( const String & SubjectName, - const String & SegmentName ) const; - /// Return the static pose translation of a subject segment. - /// - /// The static translation of the segment corresponds to the PRE-POSITION element of the segment in the subject vsk. - /// It is the base position of the segment, and is included in the value returned by GetLocalTranslation. - /// If you are required to calculate the amount a segment has moved from its base position, subtract this value from the local - /// translation. - /// - /// See Also: GetSegmentStaticRotationHelical(), GetSegmentStaticRotationMatrix(), GetSegmentStaticRotationQuaternion(), GetSegmentStaticRotationEulerXYZ(), GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// COutput_GetSegmentStaticTranslation _Output_GetSegmentStaticTranslation; - /// Client_GetSegmentStaticTranslation(pClient, "Alice", "Pelvis", &_Output_GetSegmentStaticTranslation); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentStaticTranslation Output = - /// MyClient.GetSegmentStaticTranslation( "Alice", "Pelvis" ); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentStaticTranslation( "Alice", "Pelvis" ); - /// - /// .NET example - /// - /// ViconDataStramSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentStaticTranslation Output = - /// MyClient.GetSegmentStaticTranslations( "Alice", "Pelvis" ); - /// ----- - /// \param SubjectName The name of the subject - /// \param SegmentName The name of the segment - /// \return An Output_GetSegmentStaticTranslation class containing the result of the operation and the translation of the segment. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - Output_GetSegmentStaticTranslation GetSegmentStaticTranslation( const String & SubjectName, - const String & SegmentName ) const; - /// Return the static pose rotation of a subject segment in helical coordinates. - /// The helical coordinates represent a vector whose length is the amount of rotation in radians, and the direction is the axis about which to rotate. - /// - /// The static rotation of the segment corresponds to the PRE-ORIENTATION element of the segment in the subject vsk. - /// It is the base rotation of the segment, and is included in the value returned by GetLocalRotation*. - /// If you are required to calculate the amount a segment has rotated from its base position, subtract this value from the local - /// rotation. - /// - /// See Also: GetSegmentStaticTranslation(), GetSegmentStaticRotationMatrix(), GetSegmentStaticRotationQuaternion(), GetSegmentStaticRotationEulerXYZ(), GetSegmentLocalTranslation, GetSegmentLocalRotationHelical, GetSegmentLocalRotationMatrix, GetSegmentLocalRotationQuaternion, GetSegmentLocalRotationEulerXYZ - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// COutput_GetSegmentStaticRotationHelical _Output_GetSegmentStaticRotationHelical; - /// Client_GetSegmentStaticRotationHelical( - /// pClient, "Alice", "Pelvis", &_Output_GetSegmentStaticRotationHelical); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentStaticRotationHelical Output = - /// MyClient.GetSegmentStaticRotationHelical( "Alice", "Pelvis" ); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentStaticRotationHelical( "Alice", "Pelvis" ); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentStaticRotationHelical Output = - /// MyClient.GetSegmentStaticRotationHelical( "Alice", "Pelvis" ); - /// ----- - /// \param SubjectName The name of the subject - /// \param SegmentName The name of the segment - /// \return An Output_GetSegmentStaticRotationHelical class containing the result of the operation and the rotation of the segment. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - Output_GetSegmentStaticRotationHelical GetSegmentStaticRotationHelical( const String & SubjectName, - const String & SegmentName ) const; - /// Return the static pose rotation of a subject segment as a 3x3 row-major matrix. - /// - /// The static rotation of the segment corresponds to the PRE-ORIENTATION element of the segment in the subject vsk. - /// It is the base rotation of the segment, and is included in the value returned by GetLocalRotation*. - /// If you are required to calculate the amount a segment has rotated from its base position, subtract this value from the local - /// rotation. - /// - /// See Also: GetSegmentStaticTranslation(), GetSegmentStaticRotationHelical(), GetSegmentStaticRotationQuaternion(), GetSegmentStaticRotationEulerXYZ(), GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// COutput_GetSegmentStaticRotationMatrix _Output_GetSegmentStaticRotationMatrix; - /// Client_GetSegmentStaticRotationMatrix(pClient, "Alice", "Pelvis", &_Output_GetSegmentStaticRotationMatrix); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentStaticRotationMatrix Output = - /// MyClient.GetSegmentStaticRotationMatrix( "Alice", "Pelvis" ); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentStaticRotationMatrix( "Alice", "Pelvis" ); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentStaticRotationMatrix Output = - /// MyClient.GetSegmentStaticRotationMatrix( "Alice", "Pelvis" ); - /// ----- - /// \param SubjectName The name of the subject - /// \param SegmentName The name of the segment - /// \return An Output_GetSegmentStaticRotationMatrix class containing the result of the operation and the rotation of the segment. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - Output_GetSegmentStaticRotationMatrix GetSegmentStaticRotationMatrix( const String & SubjectName, - const String & SegmentName ) const; - /// Return the static pose rotation of a subject segment in quaternion coordinates. - /// The quaternion is of the form (x, y, z, w) where w is the real component and x, y and z are the imaginary components. - /// N.B. This is different from that used in many other applications, which use (w, x, y, z). - /// - /// The static rotation of the segment corresponds to the PRE-ORIENTATION element of the segment in the subject vsk. - /// It is the base rotation of the segment, and is included in the value returned by GetLocalRotation*. - /// If you are required to calculate the amount a segment has rotated from its base position, subtract this value from the local - /// rotation. - /// - /// See Also: GetSegmentStaticTranslation(), GetSegmentStaticRotationHelical(), GetSegmentStaticRotationMatrix(), GetSegmentStaticRotationEulerXYZ(), GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// COutput_GetSegmentStaticRotationQuaternion _Output_GetSegmentStaticRotationQuaternion; - /// Client_GetSegmentStaticRotationQuaternion( - /// pClient, "Alice", "Pelvis", &_Output_GetSegmentStaticRotationQuaternion); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentStaticRotationQuaternion Output = - /// MyClient.GetSegmentStaticRotationQuaternion( "Alice", "Pelvis" ); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentStaticRotationQuaternion( "Alice", "Pelvis" ); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentStaticRotationQuaternion Output = - /// MyClient.GetSegmentStaticRotationQuaternion( "Alice", "Pelvis" ); - /// ----- - /// \param SubjectName The name of the subject - /// \param SegmentName The name of the segment - /// \return An Output_GetSegmentStaticRotationQuaternion class containing the result of the operation and the rotation of the segment. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - Output_GetSegmentStaticRotationQuaternion GetSegmentStaticRotationQuaternion( const String & SubjectName, - const String & SegmentName ) const; - - /// Return the static pose rotation of a subject segment in Euler XYZ coordinates. - /// - /// The static rotation of the segment corresponds to the PRE-ORIENTATION element of the segment in the subject vsk. - /// It is the base rotation of the segment, and is included in the value returned by GetLocalRotation*. - /// If you are required to calculate the amount a segment has rotated from its base position, subtract this value from the local - /// rotation. - /// - /// See Also: GetSegmentStaticTranslation(), GetSegmentStaticRotationHelical(), GetSegmentStaticRotationMatrix(), - /// GetSegmentStaticRotationQuaternion(), GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), - /// GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ(). - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// COutput_GetSegmentStaticRotationEulerXYZ _Output_GetSegmentStaticRotationEulerXYZ; - /// Client_GetSegmentStaticRotationEulerXYZ( - /// pClient, "Alice", "Pelvis", &_Output_GetSegmentStaticRotationEulerXYZ); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentStaticRotationEulerXYZ Output; - /// Output = MyClient.GetSegmentStaticRotationEulerXYZ( "Alice", "Pelvis" ); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentStaticRotationEulerXYZ( "Alice", "Pelvis" ); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentStaticRotationEulerXYZ Output = - /// MyClient.GetSegmentStaticRotationEulerXYZ( "Alice", "Pelvis" ); - /// ----- - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentStaticRotationEulerXYZ class containing the result of the request - /// and the rotation of the segment \f$(x,y,z)\f$. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - Output_GetSegmentStaticRotationEulerXYZ GetSegmentStaticRotationEulerXYZ( const String & SubjectName, - const String & SegmentName ) const; - /// Return the translation of a subject segment in global coordinates. - /// The translation is of the form ( x, y, z ) where x, y and z are in millimeters with respect to the global origin. - /// - /// See Also: GetSegmentGlobalRotationHelical(), GetSegmentGlobalRotationMatrix(), GetSegmentGlobalRotationQuaternion(), GetSegmentGlobalRotationEulerXYZ(), GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// COutput_GetSegmentGlobalTranslation _Output_GetSegmentGlobalTranslation; - /// Client_GetSegmentGlobalTranslation(pClient, "Alice", "Pelvis", &_Output_GetSegmentGlobalTranslation); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentGlobalTranslation Output = - /// MyClient.GetSegmentGlobalTranslation( "Alice", "Pelvis" ); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentGlobalTranslation( "Alice", "Pelvis" ); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = - /// new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentGlobalTranslation Output = - /// MyClient.GetSegmentGlobalTranslations( "Alice", "Pelvis" ); - /// ----- - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentGlobalTranslation class containing the result of the operation, the translation of the segment, and whether the segment is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - /// - Occluded will be True if the segment was absent at this frame. In this case the translation will be [0,0,0]. - Output_GetSegmentGlobalTranslation GetSegmentGlobalTranslation( const String & SubjectName, - const String & SegmentName ) const; - /// Return the rotation of a subject segment in global helical coordinates. - /// - /// See Also: GetSegmentGlobalTranslation(), GetSegmentGlobalRotationMatrix(), GetSegmentGlobalRotationQuaternion(), GetSegmentGlobalRotationEulerXYZ(), GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// COutput_GetSegmentGlobalRotationHelical _Output_GetSegmentGlobalRotationHelical; - /// Client_GetSegmentGlobalRotationHelical( - /// pClient, "Alice", "Pelvis", &_Output_GetSegmentGlobalRotationHelical); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.GetFrame(); - /// Output_GetSegmentGlobalRotationHelical Output = - /// MyClient.GetSegmentGlobalRotationHelical( "Alice", "Pelvis" ); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentGlobalRotationHelical( "Alice", "Pelvis" ); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.GetFrame(); - /// Output_GetSegmentGlobalRotationHelical Output = - /// MyClient.GetSegmentGlobalRotationHelical( "Alice", "Pelvis" ); - /// ----- - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentGlobalRotationHelical class containing the result of the operation, the rotation of the segment, and whether the segment is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - /// - Occluded will be True if the segment was absent at this frame. In this case, the rotation will be [0,0,0]. - Output_GetSegmentGlobalRotationHelical GetSegmentGlobalRotationHelical( const String & SubjectName, - const String & SegmentName ) const; - - /// Return the rotation of a subject segment as a 3x3 row-major matrix in global coordinates. - /// - /// See Also: GetSegmentGlobalTranslation(), GetSegmentGlobalRotationHelical(), GetSegmentGlobalRotationQuaternion(), GetSegmentGlobalRotationEulerXYZ(), GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ() - /// - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// COutput_GetSegmentGlobalRotationMatrix _Output_GetSegmentGlobalRotationMatrix; - /// Client_GetSegmentGlobalRotationMatrix(pClient, "Alice", "Pelvis", &_Output_GetSegmentGlobalRotationMatrix); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.GetFrame(); - /// Output_GetSegmentGlobalRotationMatrix Output = - /// MyClient.GetSegmentGlobalRotationMatrix( "Alice", "Pelvis" ); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentGlobalRotationMatrix( "Alice", "Pelvis" ); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.GetFrame(); - /// Output_GetSegmentGlobalRotationMatrix Output = - /// MyClient.GetSegmentGlobalRotationMatrix( "Alice", "Pelvis" ); - /// ----- - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentGlobalRotationMatrix Class containing the result of the operation, the rotation of the segment, and whether the segment is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - /// - Occluded will be True if the segment was absent at this frame. - Output_GetSegmentGlobalRotationMatrix GetSegmentGlobalRotationMatrix( const String & SubjectName, - const String & SegmentName ) const; - /// Return the rotation of a subject segment in global quaternion coordinates. - /// The quaternion is of the form (x, y, z, w) where w is the real component and x, y and z are the imaginary components. - /// N.B. This is different from that used in many other applications, which use (w, x, y, z). - /// - /// See Also: GetSegmentGlobalTranslation(), GetSegmentGlobalRotationHelical(), GetSegmentGlobalRotationMatrix(), GetSegmentGlobalRotationEulerXYZ(), GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// COutput_GetSegmentGlobalRotationQuaternion _Output_GetSegmentGlobalRotationQuaternion; - /// Client_GetSegmentGlobalRotationQuaternion( - /// pClient, "Alice", "Pelvis", &_Output_GetSegmentGlobalRotationQuaternion); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.GetFrame(); - /// Output_GetSegmentGlobalRotationQuaternion Output = - /// MyClient.GetSegmentGlobalRotationQuaternion( "Alice", "Pelvis" ); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentGlobalRotationQuaternion( "Alice", "Pelvis" ); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.GetFrame(); - /// Output_GetSegmentGlobalRotationQuaternion Output = - /// MyClient.GetSegmentGlobalRotationQuaternion( "Alice", "Pelvis" ); - /// ----- - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentGlobalRotationQuaternion class containing the result of the operation, the rotation of the segment, and whether the segment is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - /// - Occluded will be True if the segment was absent at this frame. In this case the Rotation will be [1,0,0,0]. - Output_GetSegmentGlobalRotationQuaternion GetSegmentGlobalRotationQuaternion( const String & SubjectName, - const String & SegmentName ) const; - /// Return the rotation of a subject segment in global Euler XYZ coordinates. - /// - /// See Also: GetSegmentGlobalTranslation(), GetSegmentGlobalRotationHelical(), GetSegmentGlobalRotationMatrix(), GetSegmentGlobalRotationQuaternion(), GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// COutput_GetSegmentGlobalRotationEulerXYZ _Output_GetSegmentGlobalRotationEulerXYZ; - /// Client_GetSegmentGlobalRotationEulerXYZ( - /// pClient, "Alice", "Pelvis", &_Output_GetSegmentGlobalRotationEulerXYZ); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.GetFrame(); - /// Output_GetSegmentGlobalRotationEulerXYZ Output = - /// MyClient.GetSegmentGlobalRotationEulerXYZ( "Alice", "Pelvis" ); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentGlobalRotationEulerXYZ( "Alice", "Pelvis" ); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.GetFrame(); - /// Output_GetSegmentGlobalRotationEulerXYZ Output = - /// MyClient.GetSegmentGlobalRotationEulerXYZ( "Alice", "Pelvis" ); - /// ----- - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentGlobalRotationEulerXYZ class containing the result of the operation, - /// the rotation of the segment, and whether the segment is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - /// - Occluded will be True if the segment was absent at this frame. In this case the rotation will be [0,0,0]. - Output_GetSegmentGlobalRotationEulerXYZ GetSegmentGlobalRotationEulerXYZ( const String & SubjectName, - const String & SegmentName ) const; - - /// Return the translation of a subject segment in local coordinates relative to its parent segment. - /// - /// See Also: GetSegmentLocalRotationHelical(), GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ(), GetSegmentGlobalTranslation(), GetSegmentGlobalRotationHelical(), GetSegmentGlobalRotationMatrix(), GetSegmentGlobalRotationQuaternion(), GetSegmentGlobalRotationEulerXYZ() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// COutput_GetSegmentLocalTranslation _Output_GetSegmentLocalTranslation; - /// Client_GetSegmentLocalTranslation(pClient, "Alice", "Pelvis", &_Output_GetSegmentLocalTranslation); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentLocalTranslation Output = - /// MyClient.GetSegmentLocalTranslation( "Alice", "Pelvis" ); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentLocalTranslation( "Alice", "Pelvis" ); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentLocalTranslation Output = - /// MyClient.GetSegmentLocalTranslations( "Alice", "Pelvis" ); - /// ----- - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentLocalTranslation class containing the result of the operation, the translation of the segment, and whether the segment is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - /// - Occluded will be True if the segment was absent at this frame. In this case the translation will be [0,0,0]. - Output_GetSegmentLocalTranslation GetSegmentLocalTranslation( const String & SubjectName, - const String & SegmentName ) const; - - /// Return the rotation of a subject segment in local helical coordinates relative to its parent segment. - /// - /// See Also: GetSegmentLocalTranslation(), GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ(), GetSegmentGlobalTranslation(), GetSegmentGlobalRotationHelical(), GetSegmentGlobalRotationMatrix(), GetSegmentGlobalRotationQuaternion(), GetSegmentGlobalRotationEulerXYZ() - /// - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// COutput_GetSegmentLocalRotationHelical _Output_GetSegmentLocalRotationHelical; - /// Client_GetSegmentLocalRotationHelical( - /// pClient, "Alice", "Pelvis", &_Output_GetSegmentLocalRotationHelical); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.GetFrame(); - /// Output_GetSegmentLocalRotationHelical Output = - /// MyClient.GetSegmentLocalRotationHelical( "Alice", "Pelvis" ); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentLocalRotationHelical( "Alice", "Pelvis" ); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.GetFrame(); - /// Output_GetSegmentLocalRotationHelical Output = - /// MyClient.GetSegmentLocalRotationHelical( "Alice", "Pelvis" ); - /// ----- - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentLocalRotationHelical class containing the result of the operation, - /// the rotation of the segment, and whether the segment is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - /// - Occluded will be True if the segment was absent at this frame. In this case the Rotation will be [0,0,0]. - Output_GetSegmentLocalRotationHelical GetSegmentLocalRotationHelical( const String & SubjectName, - const String & SegmentName ) const; - - /// Return the rotation row-major matrix of a subject segment in local coordinates relative to its parent segment. - /// - /// See Also: GetSegmentLocalTranslation(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ(), GetSegmentGlobalTranslation(), GetSegmentGlobalRotationHelical(), GetSegmentGlobalRotationMatrix() , GetSegmentGlobalRotationQuaternion(), GetSegmentGlobalRotationEulerXYZ() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// COutput_GetSegmentLocalRotationMatrix _Output_GetSegmentLocalRotationMatrix; - /// Client_GetSegmentLocalRotationMatrix( - /// pClient, "Alice", "Pelvis", &_Output_GetSegmentLocalRotationMatrix); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.GetFrame(); - /// Output_GetSegmentLocalRotationMatrix Output = - /// MyClient.GetSegmentLocalRotationMatrix( "Alice", "Pelvis" ); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentLocalRotationMatrix( "Alice", "Pelvis" ); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.GetFrame(); - /// Output_GetSegmentLocalRotationMatrix Output = - /// MyClient.GetSegmentLocalRotationMatrix( "Alice", "Pelvis" ); - /// ----- - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentLocalRotationMatrix class containing the result of the operation, - /// the rotation of the segment, and whether the segment is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - /// - Occluded will be True if the segment was absent at this frame. - Output_GetSegmentLocalRotationMatrix GetSegmentLocalRotationMatrix( const String & SubjectName, - const String & SegmentName ) const; - /// Return the rotation of a subject segment in local quaternion coordinates relative to its parent segment. - /// The quaternion is of the form (x, y, z, w) where w is the real component and x, y and z are the imaginary components. N.B. This is different from that used in many other applications, which use (w, x, y, z). - /// - /// See Also: GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationEulerXYZ(), GetSegmentGlobalTranslation(), GetSegmentGlobalRotationHelical(), GetSegmentGlobalRotationMatrix(), GetSegmentGlobalRotationQuaternion(), GetSegmentGlobalRotationEulerXYZ() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// COutput_GetSegmentLocalRotationQuaternion _Output_GetSegmentLocalRotationQuaternion; - /// Client_GetSegmentLocalRotationQuaternion( - /// pClient, "Alice", "Pelvis", &_Output_GetSegmentLocalRotationQuaternion); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.GetFrame(); - /// Output_GetSegmentLocalRotationQuaternion Output = - /// MyClient.GetSegmentLocalRotationQuaternion( "Alice", "Pelvis" ); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentLocalRotationQuaternion( "Alice", "Pelvis" ); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.GetFrame(); - /// Output_GetSegmentLocalRotationQuaternion Output = - /// MyClient.GetSegmentLocalRotationQuaternion( "Alice", "Pelvis" ); - /// ----- - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentLocalRotationQuaternion class containing the result of the operation, - /// the rotation of the segment, and whether the segment is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - /// - Occluded will be True if the segment was absent at this frame. In this case the rotation will be [1,0,0,0]. - Output_GetSegmentLocalRotationQuaternion GetSegmentLocalRotationQuaternion( const String & SubjectName, - const String & SegmentName ) const; - - /// Return the rotation of a subject segment in local Euler XYZ coordinates relative to its parent segment. - /// - /// See Also: GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationQuaternion(), GetSegmentGlobalTranslation(), GetSegmentGlobalRotationHelical(), GetSegmentGlobalRotationMatrix() , GetSegmentGlobalRotationQuaternion(), GetSegmentGlobalRotationEulerXYZ() - /// - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// COutput_GetSegmentLocalRotationEulerXYZ _Output_GetSegmentLocalRotationEulerXYZ; - /// Client_GetSegmentLocalRotationEulerXYZ( - /// pClient, "Alice", "Pelvis", &_Output_GetSegmentLocalRotationEulerXYZ); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.GetFrame(); - /// Output_GetSegmentLocalRotationEulerXYZ Output = - /// MyClient.GetSegmentLocalRotationEulerXYZ( "Alice", "Pelvis" ); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentLocalRotationEulerXYZ( "Alice", "Pelvis" ); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.GetFrame(); - /// Output_GetSegmentLocalRotationEulerXYZ Output = - /// MyClient.GetSegmentLocalRotationEulerXYZ( "Alice", "Pelvis" ); - /// ----- - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentLocalRotationEulerXYZ class containing the result of the operation, - /// the rotation of the segment, and whether the segment is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - /// - Occluded will be True if the segment was absent at this frame. In this case the rotation will be [0,0,0]. - Output_GetSegmentLocalRotationEulerXYZ GetSegmentLocalRotationEulerXYZ( const String & SubjectName, - const String & SegmentName ) const; - - /// Return the quality score for a specified Object (Subject). This is only implemented by Tracker. - /// - /// See Also: GetSubjectCount(), GetSubjectName() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_EnableSegmentData( pClient ); - /// Client_Connect( pClient, "localhost" ); - /// COutput_GetObjectQuality _Output_GetObjectQuality; - /// Client_GetObjectQuality(pClient, "Object", &_Output_GetObjectQuality); - /// // _output_GetObjectQuality.Result = NoFrame - /// // _output_GetObjectQuality.Quality = 0 - /// Client_GetFrame( pClient ); - /// Client_GetObjectQuality(pClient, "Object", &_Output_GetObjectQuality); - /// // _output_GetObjectQuality.Result = Success - /// // _output_GetObjectQuality.Quality >= 0.0 && _output_GetObjectQuality.Quality <= 1.0 - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.EnableSegmentData(); - /// MyClient.Connect( "localhost" ); - /// Output_GetObjectQuality Output; - /// Output = MyClient.GetObjectQuality( "Object" ); - /// // Output.Result == NoFrame - /// // Output.Quality == 0 - /// MyClient.GetFrame(); - /// Output = MyClient.GetMarkerCount( "Camera" ); - /// // Output.Result == InvalidSubjectName - /// // Output.Quality == 0 - /// // (no "Camera") - /// Output = MyClient.GetMarkerCount( "Object" ); - /// // Output.Result == Success - /// // Output.Quality >= 0.0 && Output.Quality <= 1.0 - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.EnableSegmentData (); - /// MyClient.Connect( "localhost" ); - /// Output = MyClient.GetObjectQuality( "Object" ); - /// % Output.Result == NoFrame - /// % Output.Quality == 0 - /// MyClient.GetFrame(); - /// Output = MyClient.GetObjectQuality( "Camera" ); - /// % Output.Result == InvalidSubjectName - /// % Output.Quality == 0 - /// % (no "Camera") - /// Output = MyClient.GetObjectQuality( "Object" ); - /// % Output.Result == Success - /// % Output.Quality >= 0 && Output.Quality >= 1.0 - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.EnableSegmentData (); - /// MyClient.Connect( "localhost" ); - /// Output_GetMarkerCount Output; - /// Output = MyClient.GetObjectQuality( "Object" ); - /// // Output.Result == NoFrame - /// // Output.Quality == 0 - /// MyClient.GetFrame(); - /// Output = MyClient.GetObjectQuality( "Camera" ); - /// // Output.Result == InvalidSubjectName - /// // Output.Quality == 0 - /// // (no "Camera") - /// Output = MyClient.GetObjectQuality( "Object" ); - /// // Output.Result == Success - /// // Output.Quality >= 0 && Output.Quality >= 1.0 - /// ----- - /// \param ObjectName The name of the subject. - /// \return An Output_GetObjectQuality class containing the result of the operation and the quality score of the object. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - Output_GetObjectQuality GetObjectQuality( const String & ObjectName ) const; - - /// Return the number of markers for a specified subject in the DataStream. This information can be used in conjunction with GetMarkerName. - /// - /// See Also: GetSubjectName(), GetMarkerName() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableMarkerData( pClient ); - /// COutput_GetMarkerCount MarkerCount; - /// Client_GetMarkerCount(pClient, "Bob", &MarkerCount); - /// // Output.Result = NoFrame - /// // Output.MarkerCount = 0 - /// Client_GetFrame( pClient ); - /// Client_GetMarkerCount(pClient, "Bob", &MarkerCount); - /// // Output.Result = Success - /// // Output.MarkerCount >= 0 - /// Client_GetMarkerCount(pClient, "Alice", &MarkerCount); - /// // (no "Alice") - /// // Output.Result = InvalidSubjectName - /// // Output.MarkerCount == 0 - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// CPP::Client MyClient; - /// MyClient.EnableMarkerData(); - /// MyClient.Connect( "localhost" ); - /// Output_GetMarkerCount Output; - /// Output = MyClient.GetMarkerCount( "Bob" ); // Output.Result == NoFrame - /// // Output.MarkerCount == 0 - /// MyClient.GetFrame(); - /// Output = MyClient.GetMarkerCount( "Alice" ); - /// // Output.Result == InvalidSubjectName - /// // Output.MarkerCount == 0 - /// // (no "Alice") - /// Output = MyClient.GetMarkerCount( "Bob" ); // Output.Result == Success - /// // Output.MarkerCount >= 0 - /// - /// MATLAB example - /// - /// // MyClient = Client(); - /// MyClient.EnableMarkerData(); - /// MyClient.Connect( "localhost" ); - /// Output = MyClient.GetMarkerCount( "Bob" ); % Output.Result == NoFrame - /// % Output.MarkerCount == 0 - /// MyClient.GetFrame(); - /// Output = MyClient.GetMarkerCount( "Alice" ); - /// % Output.Result == InvalidSubjectName - /// % Output.MarkerCount == 0 - /// % (no "Alice") - /// Output = MyClient.GetMarkerCount( "Bob" ); % Output.Result == Success - /// % Output.MarkerCount >= 0 - /// - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = - /// new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.EnableMarkerData(); - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// Output_GetMarkerCount Output; - /// Output = MyClient.GetMarkerCount( "Bob" ); // Output.Result == NoFrame - /// // Output.MarkerCount == 0 - /// MyClient.GetFrame(); - /// Output = MyClient.GetMarkerCount( "Alice" ); - /// // Output.Result == InvalidSubjectName - /// // Output.MarkerCount == 0 - /// // (no "Alice") - /// Output = MyClient.GetMarkerCount( "Bob" ); // Output.Result == Success - /// // Output.MarkerCount >= 0 - /// ----- - /// \param SubjectName The name of the subject. - /// \return An Output_GetMarkerCount class containing the result of the operation, and the number of markers. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - Output_GetMarkerCount GetMarkerCount( const String & SubjectName ) const; - - /// Return the name of a marker for a specified subject. This can be passed into GetMarkerGlobalTranslation. - /// - /// See Also: GetMarkerCount(), GetMarkerGlobalTranslation() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableMarkerData( pClient ); - /// Client_GetFrame( pClient ); - /// COutput_GetMarkerCount MarkerCount; - /// Client_GetMarkerCount(pClient, "Bob", &MarkerCount); - /// // MarkerCount.Result == Success - /// // MarkerCount.MarkerCount == 2 - /// A valid Marker Index is between 0 and GetMarkerCount()-1 - /// char MarkerName[128]; - /// Client_GetMarkerName(pClient, "Alice", 0, 128, MarkerName); - /// // MarkerName.Result == InvalidSubjectName - /// // MarkerName.MarkerName == "" - /// // (no "Alice") - /// Client_GetMarkerName(pClient, "Bob", 0, 128, MarkerName); - /// // MarkerName.Result == Success - /// // MarkerName.MarkerName == "LASI" - /// Client_GetMarkerName(pClient, "Bob", 1, 128, MarkerName); - /// // MarkerName.Result == Success - /// // MarkerName.MarkerName == "RASI" - /// Client_GetMarkerName(pClient, "Bob", 2, 128, MarkerName); - /// // MarkerName.Result == InvalidIndex - /// // MarkerName.MarkerName == "" - /// // (no third marker) - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// A valid Marker Index is between 0 and GetMarkerCount()-1 - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableMarkerData(); - /// MyClient.GetFrame(); - /// Output_GetMarkerCount OutputGMC; - /// OutputGMC = MyClient.GetMarkerCount( "Bob" ); - /// // OutputGMC.Result == Success - /// // OutputGMC.MarkerCount == 2 - /// Output_GetMarkerName OutputGMN; - /// OutputGMN = MyClient.GetMarkerName( "Alice", 0 ); - /// // OutputGMN.Result == InvalidSubjectName - /// // OutputGMN.MarkerName == "" - /// // (no "Alice") - /// OutputGMN = MyClient.GetMarkerName( "Bob", 0 ); - /// // OutputGMN.Result == Success - /// // OutputGMN.MarkerName == "LASI" - /// OutputGMN = MyClient.GetMarkerName( "Bob", 1 ); - /// // OutputGMN.Result == Success - /// // OutputGMN.MarkerName == "RASI" - /// OutputGMN = MyClient.GetMarkerName( "Bob", 2 ); - /// // OutputGMN.Result == InvalidIndex - /// // OutputGMN.MarkerName == "" - /// // (no third marker) - /// - /// MATLAB example - /// - /// A valid Marker Index is between 1 and GetMarkerCount() - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableMarkerData(); - /// MyClient.GetFrame(); - /// OutputGMC = MyClient.GetMarkerCount( "Bob" ); - /// // OutputGMC.Result == Success - /// // OutputGMC.MarkerCount == 2 - /// OutputGMN = MyClient.GetMarkerName( "Alice", 1 ); - /// // OutputGMN.Result == InvalidSubjectName - /// // OutputGMN.MarkerName == "" - /// // (no "Alice") - /// OutputGMN = MyClient.GetMarkerName( "Bob", 1 ); - /// // OutputGMN.Result == Success - /// // OutputGMN.MarkerName == "LASI" - /// OutputGMN = MyClient.GetMarkerName( "Bob", 2 ); - /// // OutputGMN.Result == Success - /// // OutputGMN.MarkerName == "RASI" - /// OutputGMN = MyClient.GetMarkerName( "Bob", 3 ); - /// // OutputGMN.Result == InvalidIndex - /// // OutputGMN.MarkerName == "" - /// // (no third marker) - /// - /// .NET example - /// - /// A valid Marker Index is between 0 and GetMarkerCount()-1 - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableMarkerData(); - /// MyClient.GetFrame(); - /// Output_GetMarkerCount OutputGMC; - /// OutputGMC = MyClient.GetMarkerCount( "Bob" ); - /// // OutputGMC.Result == Success - /// // OutputGMC.MarkerCount == 2 - /// Output_GetMarkerName OutputGMN; - /// OutputGMN = MyClient.GetMarkerName( "Alice", 0 ); - /// // OutputGMN.Result == InvalidSubjectName - /// // OutputGMN.MarkerName == "" - /// // (no "Alice") - /// OutputGMN = MyClient.GetMarkerName( "Bob", 0 ); - /// // OutputGMN.Result == Success - /// // OutputGMN.MarkerName == "LASI" - /// OutputGMN = MyClient.GetMarkerName( "Bob", 1 ); - /// // OutputGMN.Result == Success - /// // OutputGMN.MarkerName == "RASI" - /// OutputGMN = MyClient.GetMarkerName( "Bob", 2 ); - /// // OutputGMN.Result == InvalidIndex - /// // OutputGMN.MarkerName == "" - /// // (no third marker) - /// ----- - /// \param SubjectName The name of the subject. - /// \param MarkerIndex The index of the marker. - /// \return An Output_GetMarkerName class containing the result of the operation and the name of the marker. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidIndex - Output_GetMarkerName GetMarkerName( const String & SubjectName, - const unsigned int MarkerIndex ) const; - - /// Return the name of the segment that is the parent of this marker. - /// - /// See Also: GetMarkerCount(), GetMarkerName(), GetMarkerGlobalTranslation() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableMarkerData( pClient ); - /// Client_GetFrame( pClient ); - /// char MarkerParentName[128]; - /// CEnum Result = Client_GetMarkerParentName(pClient, "Bob", "LFHD", 128, MarkerParentName); - /// // Result == Success - /// // MarkerParentName == "Head" - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableMarkerData(); - /// MyClient.GetFrame(); - /// Output_GetMarkerParentName Output; - /// Output = MyClient.GetMarkerParentName( "Bob", "LFHD" ); - /// // Output.Result == Success - /// // Output.SegmentName == "Head" - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableMarkerData(); - /// MyClient.GetFrame(); - /// Output = MyClient.GetMarkerParentName( "Bob", "LFHD" ); - /// // Output.Result == Success - /// // Output.SegmentName == "Head" - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableMarkerData(); - /// MyClient.GetFrame(); - /// Output_GetMarkerParentName Output; - /// Output = MyClient.GetMarkerParentName( "Bob", "LFHD" ); - /// // Output.Result == Success - /// // Output.SegmentName == "Head" - /// ----- - /// \param SubjectName The name of the subject. - /// \param MarkerName The name of the marker. - /// \return An Output_GetMarkerParentName class containing the result of the operation and the name of the parent segment. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidMarkerName - Output_GetMarkerParentName GetMarkerParentName( const String & SubjectName, - const String & MarkerName ) const; - - /// Return the translation of a subject marker in global coordinates. - /// The Translation is of the form ( x, y, z ) where x, y and z are in millimeters with respect to the global origin. - /// - /// See Also: GetMarkerName() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableMarkerData( pClient ); - /// Client_GetFrame( pClient ); - /// COutput_GetMarkerGlobalTranslation _Output_GetMarkerGlobalTranslation; - /// Client_GetMarkerGlobalTranslation(pClient, "Alice", "LASI", &_Output_GetMarkerGlobalTranslation); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableMarkerData(); - /// MyClient.GetFrame(); - /// Output_GetMarkerGlobalTranslation Output = - /// MyClient.GetMarkerGlobalTranslation( "Alice", "LASI" ); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableMarkerData(); - /// MyClient.GetFrame(); - /// Output = MyClient.GetMarkerGlobalTranslation( "Alice", "LASI" ); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableMarkerData(); - /// MyClient.GetFrame(); - /// Output_GetMarkerGlobalTranslation Output = - /// MyClient.GetMarkerGlobalTranslation( "Alice", "LASI" ); - /// ----- - /// \param SubjectName The name of the subject. - /// \param MarkerName The name of the marker. - /// \return An Output_GetMarkerGlobalTranslation class containing the result of the operation, the translation of the marker, and whether the marker is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidMarkerName - /// - Occluded will be true if the marker was absent at this frame. In this case the Translation will be [0,0,0]. - Output_GetMarkerGlobalTranslation GetMarkerGlobalTranslation( const String & SubjectName, - const String & MarkerName ) const; - - /// Return the number of rays that are contributing to a labeled marker in the DataStream. This information can be used in conjunction with GetMarkerRayContribution. - /// - /// See Also: GetMarkerRayContribution(), EnableMarkerRayData(), DisableMarkerRayData(), IsMarkerRayDataEnabled() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableMarkerRayData( pClient ); - /// Client_GetFrame( pClient ); - /// COutput_GetMarkerRayContributionCount _Output_GetMarkerRayContributionCount; - /// Client_GetMarkerRayContributionCount(pClient, "Alice", "LASI", &_Output_GetMarkerRayContributionCount); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableMarkerRayData(); - /// MyClient.GetFrame(); - /// Output_GetMarkerRayContributionCount Output = - /// MyClient.GetMarkerRayContributionCount ( "Alice", "LASI" ); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableMarkerRayData(); - /// MyClient.GetFrame(); - /// Output = MyClient.GetMarkerRayContributionCount ( "Alice", "LASI" ); - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableMarkerRayData(); - /// MyClient.GetFrame(); - /// Output_GetMarkerRayContributionCount Output = - /// MyClient.GetMarkerRayContributionCount( "Alice", "LASI" ); - /// ----- - /// \param SubjectName The name of the subject. - /// \param MarkerName The name of the marker. - /// \return An Output_GetMarkerRayContributionCount class containing the result of the operation and the number of rays. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidMarkerName - Output_GetMarkerRayContributionCount GetMarkerRayContributionCount( const String & SubjectName, - const String & MarkerName ) const; - /// Return the camera ID for an indexed ray that is contributing to a labeled marker in the DataStream. This information can be used in conjunction with GetMarkerRayContributionCount. - /// - /// See Also: GetMarkerRayContributionCount(), EnableMarkerRayData(), DisableMarkerRayData(), IsMarkerRayDataEnabled() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableMarkerData( pClient ); - /// Client_GetFrame( pClient ); - /// COutput_GetMarkerRayContribution _Output_GetMarkerRayContribution; - /// Client_GetMarkerRayContribution(pClient, "Alice", "LASI", 0, &_Output_GetMarkerRayContribution); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// A valid Ray Index is between 0 and GetMarkerRayContributionCount()-1 - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.EnableMarkerRayData(); - /// MyClient.GetFrame(); - /// Output_GetMarkerRayContribution Output = - /// MyClient.GetMarkerRayContribution( "Alice", "LASI", 0 ); - /// - /// MATLAB example - /// - /// A valid Ray Index is between 1 and GetMarkerRayContributionCount() - /// MarkerRayContributionIndex ) - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.EnableMarkerRayData(); - /// MyClient.GetFrame(); - /// Output = MyClient.GetMarkerRayContribution ( "Alice", "LASI", 1 ); - /// - /// .NET example - /// - /// A valid Ray Index is between 0 and GetMarkerRayContributionCount()-1 - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.EnableMarkerRayData(); - /// MyClient.GetFrame(); - /// Output_GetMarkerRayContribution Output = - /// MyClient.GetMarkerRayContribution( "Alice", "LASI", 0 ); - /// ----- - /// \param SubjectName The name of the subject. - /// \param MarkerName The name of the marker. - /// \param MarkerRayContributionIndex The index of the ray required. - /// \return An Output_GetMarkerRayContribution class containing the result of the operation, the camera ID of the camera producing the ray and the index of the centroid resulting from the ray. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidMarkerName - Output_GetMarkerRayContribution GetMarkerRayContribution( const String & SubjectName, - const String & MarkerName, - unsigned int MarkerRayContributionIndex ) const; - - /// Return the number of unlabeled markers in the DataStream. This information can be used in conjunction with GetGlobalUnlabeledMarkerTranslation - /// - /// See Also: GetGlobalUnlabeledMarkerTranslation() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableUnlabeledMarkerData( pClient ); - /// Client_GetFrame( pClient ); - /// COutput_GetUnlabeledMarkerCount UnlabeledMarkerCount; - /// Client_GetUnlabeledMarkerCount(pClient, &UnlabeledMarkerCount); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.EnableUnlabeledMarkerData(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetUnlabeledMarkerCount Output = - /// MyClient.GetUnlabeledMarkerCount(); // Output.Result == Success - /// // Output.MarkerCount >= 0 - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.EnableUnlabeledMarkerData(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetUnlabeledMarkerCount(); // Output.Result == Success - /// // Output.MarkerCount >= 0 - /// - /// .NET example - /// - /// MyClient = Client(); - /// MyClient.EnableUnlabeledMarkerData(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetUnlabeledMarkerCount(); // Output.Result == Success - /// // Output.MarkerCount >= 0 - /// ----- - /// \return An Output_GetUnlabeledMarkerCount class containing the result of the operation and the number of markers. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - Output_GetUnlabeledMarkerCount GetUnlabeledMarkerCount() const; - - /// Return the translation of an unlabeled marker in global coordinates. - /// The Translation is of the form ( x, y, z ) where x, y and z are in millimeters with respect to the global origin. - /// - /// See Also: GetUnlabeledMarkerCount() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableUnlabeledMarkerData( pClient ); - /// Client_GetFrame( pClient ); - /// COutput_GetUnlabeledMarkerGlobalTranslation _Output_GetUnlabeledMarkerGlobalTranslation; - /// Client_GetUnlabeledMarkerGlobalTranslation( pClient, 0, &_Output_GetUnlabeledMarkerGlobalTranslation ); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// A valid Marker Index is between 0 and GetUnlabeledMarkerCount()-1 - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableUnlabeledMarkerData(); - /// MyClient.GetFrame(); - /// Output_GetUnlabeledMarkerGlobalTranslation Output = - /// MyClient.GetUnlabeledMarkerGlobalTranslation( 0 ); - /// - /// MATLAB example - /// - /// A valid Marker Index is between 1 and GetUnlabeledMarkerCount() - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableUnlabeledMarkerData(); - /// MyClient.GetFrame(); - /// Output = MyClient.GetUnlabeledMarkerGlobalTranslation( 1 ); - /// - /// .NET example - /// - /// A valid Marker Index is between 0 and GetUnlabeledMarkerCount()-1 - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableUnlabeledMarkerData(); - /// MyClient.GetFrame(); - /// Output_GetUnlabeledMarkerGlobalTranslation Output = - /// MyClient.GetUnlabeledMarkerGlobalTranslation( 0 ); - /// ----- - /// \param MarkerIndex The index of the marker - /// \return An Output_GetUnlabeledMarkerGlobalTranslation class containing the result of the operation and the translation of the marker. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - Output_GetUnlabeledMarkerGlobalTranslation GetUnlabeledMarkerGlobalTranslation( const unsigned int MarkerIndex ) const; - - /// Returns the number of all labeled markers in the datastream across all subjects. This may be used to determine marker index range for use with - /// GetLabeledMarkerGlobalTranslation(). - /// - /// See Also: GetLabeledMarkerGlobalTranslation() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableMarkerData( pClient ); - /// Client_GetFrame( pClient ); - /// COutput_GetLabeledMarkerCount LabeledMarkerCount; - /// Client_GetLabeledMarkerCount( pClient, &LabeledMarkerCount ); - /// // LabeledMarkerCount.Result == Success - /// // LabeledMarkerCount.Markercount >= 0 - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.EnableMarkerData(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetLabeledMarkerCount Output = - /// MyClient.GetLabeledMarkerCount(); - /// // Output.Result == Success - /// // Output.MarkerCount >= 0 - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.EnableMarkerData(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetLabeledMarkerCount(); // Output.Result == Success - /// // Output.MarkerCount >= 0 - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.EnableMarkerData(); - /// MyClient.Connect("localhost"); - /// MyClient.GetFrame(); - /// Output_GetLabeledMarkerCount Output = MyClient.GetLabeledMarkerCount(); - /// // Output.Result == Success - /// // Output.MarkerCount >= 0 - /// ----- - /// - /// \return An Output_GetLabeledMarkerCount class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - Output_GetLabeledMarkerCount GetLabeledMarkerCount() const; - - /// Return the translation of a labeled marker in global coordinates. - /// The Translation is of the form ( x, y, z ) where x, y and z are in millimeters with respect to the global origin. - /// - /// See Also: GetLabeledMarkerCount() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableMarkerData( pClient ); - /// Client_GetFrame( pClient ); - /// COutput_GetLabeledMarkerGlobalTranslation LabeledMarkerGlobalTranslation; - /// Client_GetLabeledMarkerGlobalTranslation( pClient, &LabeledMarkerGlobalTranslation ); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// A valid Marker Index is between 0 and GetLabeledMarkerCount()-1 - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableMarkerData(); - /// MyClient.GetFrame(); - /// Output_GetLabeledMarkerGlobalTranslation Output = - /// MyClient.GetLabeledMarkerGlobalTranslation( 0 ); - /// - /// MATLAB example - /// - /// A valid Marker Index is between 1 and GetUnlabeledMarkerCount() - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableMarkerData(); - /// MyClient.GetFrame(); - /// Output = MyClient.GetLabeledMarkerGlobalTranslation( 1 ); /// - /// .NET example - /// - /// A valid Marker Index is between 0 and GetLabeledMarkerCount()-1 - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableLabeledMarkerData(); - /// MyClient.GetFrame(); - /// Output_GetLabeledMarkerGlobalTranslation Output = - /// MyClient.GetLabeledMarkerGlobalTranslation( 0 ); - /// ----- - /// - /// \return An Output_GetLabeledMarkerGlobalTranslation class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - Output_GetLabeledMarkerGlobalTranslation GetLabeledMarkerGlobalTranslation( const unsigned int MarkerIndex ) const; - - /// Return the number of force plates, EMGs, and other devices in the DataStream. This information can be used in conjunction with GetDeviceName. - /// - /// See Also: GetDeviceName() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// COutput_GetDeviceCount DeviceCount; - /// Client_GetDeviceCount( pClient, &DeviceCount ); - /// // DeviceCount.Result == Success - /// // DeviceCount.DeviceCount >= 0 - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.EnableDeviceData(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetDeviceCount Output = MyClient.GetDeviceCount(); - /// // Output.Result == Success - /// // Output.DeviceCount >= 0 - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.EnableDeviceData(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetDeviceCount(); // Output.Result == Success - /// // Output.DeviceCount >= 0 - /// - /// .NET example - /// - /// ViconDataStreamSDK::DotNET::Client MyClient; - /// MyClient.EnableDeviceData(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetDeviceCount Output = MyClient.GetDeviceCount(); - /// // Output.Result == Success - /// // Output.DeviceCount >= 0 - /// ----- - /// \return An Output_GetDeviceCount class containing the result of the operation and the number of devices. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - Output_GetDeviceCount GetDeviceCount() const; - - /// Return the name and type of a device. This name can be passed into device functions. - /// - /// See Also: GetDeviceCount(), GetDeviceOutputCount(), GetDeviceOutputValue() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableDeviceData( pClient ); - /// Client_GetFrame( pClient ); - /// COutput_GetDeviceCount DeviceCount; - /// Client_GetDeviceCount( pClient, &DeviceCount ); - /// // DeviceCount.Result == Success - /// // DeviceCount.DeviceCount == 2 - /// char DeviceName[128]; - /// CEnum DeviceType; - /// CEnum Result = Client_GetDeviceName( pClient, 0, 128, DeviceName, &DeviceType ); - /// // Result == Success - /// // DeviceName == "ZeroWire" - /// // DeviceType == Unknown - /// Result = Client_GetDeviceName( pClient, 1, 128, DeviceName, &DeviceType ); - /// // Result == Success - /// // DeviceName == "AMTI #1" - /// // DeviceType == ForcePlate - /// Result = Client_GetDeviceName( pClient, 2, 128, DeviceName, &DeviceType ); - /// // Result == InvalidIndex - /// // DeviceName == "" - /// // DeviceType == Unknown - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// A valid Device Index is between 0 and GetDeviceCount()-1 - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableDeviceData(); - /// MyClient.GetFrame(); - /// Output_GetDeviceCount OutputGDC; - /// OutputGDC = MyClient.GetDeviceCount( DeviceCount ); - /// // OutputGDC.Result == Success - /// // OutputGDC.DeviceCount == 2 - /// Output_GetDeviceName OutputGDN; - /// OutputGDN = MyClient.GetDeviceName( 0 ); - /// // OutputGDN.Result == Success - /// // OutputGDN.DeviceName == "ZeroWire" - /// // OutputGDN.DeviceType == Unknown - /// OutputGDN = MyClient.GetDeviceName( 1 ); - /// // OutputGDN.Result == Success - /// // OutputGDN.DeviceName == "AMTI #1" - /// // OutputGDN.DeviceType == ForcePlate - /// OutputGDN = MyClient.GetDeviceName( 2 ); - /// // OutputGDN.Result == InvalidIndex - /// // OutputGDN.DeviceName == "" - /// // OutputGDN.DeviceType == Unknown - /// - /// MATLAB example - /// - /// A valid Device Index is between 1 and GetDeviceCount() - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableDeviceData(); - /// MyClient.GetFrame(); - /// OutputGDC = MyClient.GetDeviceCount( DeviceCount ); - /// % OutputGDC.Result == Success - /// % OutputGDC.DeviceCount == 2 - /// OutputGDN = MyClient.GetDeviceName( 1 ); - /// % OutputGDN.Result == Success - /// % OutputGDN.DeviceName == "ZeroWire" - /// % OutputGDN.DeviceType == Unknown - /// OutputGDN = MyClient.GetDeviceName( 2 ); - /// % OutputGDN.Result == Success - /// % OutputGDN.DeviceName == "AMTI #1" - /// % OutputGDN.DeviceType == ForcePlate - /// OutputGDN = MyClient.GetDeviceName( 3 ); - /// % OutputGDN.Result == InvalidIndex - /// % OutputGDN.DeviceName == "" - /// % OutputGDN.DeviceType == Unknown - /// - /// .NET example - /// - /// A valid Device Index is between 0 and GetDeviceCount()-1 - /// ViconDataStreamSDK.DotNET.Client MyClient = - /// new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableDeviceData(); - /// MyClient.GetFrame(); - /// Output_GetDeviceCount OutputGDC; - /// OutputGDC = MyClient.GetDeviceCount( DeviceCount ); - /// // OutputGDC.Result == Success - /// // OutputGDC.DeviceCount == 2 - /// Output_GetDeviceName OutputGDN; - /// OutputGDN = MyClient.GetDeviceName( 0 ); - /// // OutputGDN.Result == Success - /// // OutputGDN.DeviceName == "ZeroWire" - /// // OutputGDN.DeviceType == Unknown - /// OutputGDN = MyClient.GetDeviceName( 1 ); - /// // OutputGDN.Result == Success - /// // OutputGDN.DeviceName == "AMTI #1" - /// // OutputGDN.DeviceType == ForcePlate - /// OutputGDN = MyClient.GetDeviceName( 2 ); - /// // OutputGDN.Result == InvalidIndex - /// // OutputGDN.DeviceName == "" - /// // OutputGDN.DeviceType == Unknown - /// ----- - /// \param DeviceIndex The index of the device. - /// \return An Output_GetDeviceName class containing the result of the operation, the name of the device, and the device type. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - /// The Device Type will be: - /// + Unknown - /// + ForcePlate - Output_GetDeviceName GetDeviceName( const unsigned int DeviceIndex ) const; - - /// Return the number of outputs for a device in the DataStream. This information can be used in conjunction with GetDeviceOutputName. - /// - /// See Also: GetDeviceName(), GetDeviceOutputName() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// client_EnableDeviceData( pClient ); - /// Client_GetFrame( pClient ); - /// COutput_GetDeviceOutputCount DeviceOutputCount; - /// Client_GetDeviceOutputCount(pClient, "DataGlove", &DeviceOutputCount); - /// // DeviceOtuputCount.Result == InvalidDeviceName - /// // DeviceOtuputCount.DeviceOutputCount == 0 - /// // (no "DataGlove" device) - /// Client_GetDeviceOutputCount(pClient, "ZeroWire", &DeviceOutputCount); - /// // DeviceOtuputCount.Result == Success - /// // DeviceOtuputCount.DeviceOutputCount == 6 - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableDeviceData(); - /// MyClient.GetFrame(); - /// Output_GetDeviceOutputCount Output; - /// Output = MyClient.GetDeviceOutputCount( "DataGlove" ); - /// // Output.Result == InvalidDeviceName - /// // Output.DeviceOutputCount == 0 - /// // (no "DataGlove" device) - /// Output = MyClient.GetDeviceOutputCount( "ZeroWire" ); - /// // Output.Result == Success - /// // Output.DeviceOutputCount == 6 - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableDeviceData(); - /// MyClient.GetFrame(); - /// Output = MyClient.GetDeviceOutputCount( "DataGlove" ); - /// // Output.Result == InvalidDeviceName - /// // Output.DeviceOutputCount == 0 - /// // (no "DataGlove" device) - /// Output = MyClient.GetDeviceOutputCount( "ZeroWire" ); - /// // Output.Result == Success - /// // Output.DeviceOutputCount == 6 - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableDeviceData(); - /// MyClient.GetFrame(); - /// Output_GetDeviceOutputCount Output; - /// Output = MyClient.GetDeviceOutputCount( "DataGlove" ); - /// // Output.Result == InvalidDeviceName - /// // Output.DeviceOutputCount == 0 - /// // (no "DataGlove" device) - /// Output = MyClient.GetDeviceOutputCount( "ZeroWire" ); - /// // Output.Result == Success - /// // Output.DeviceOutputCount == 6 - /// ----- - /// \param DeviceName The device name - /// \return An Output_GetDeviceOutputCount class containing the result of the operation and the number of device outputs. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidDeviceName - Output_GetDeviceOutputCount GetDeviceOutputCount( const String & DeviceName ) const; - - /// Return the name and SI unit of a device output. This name can be passed into GetDeviceOutputValue. - /// - /// See Also: GetDeviceCount(), GetDeviceOutputCount(), GetDeviceOutputValue() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableDeviceData(); - /// Client_GetFrame( pClient ); - /// char DeviceOutputName[128]; - /// CEnum DeviceOutputUnit; - /// CEnum Result = Client_GetDeviceOutputName(pClient, "AMTI", 0, 128, DeviceOutputName, &DeviceOutputUnit); - /// // Result == Success - /// // DeviceOutputName == "Fx" - /// // DeviceOutputUnit == Newton - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// A valid Device Output Index is between 0 and GetDeviceOutputCount()-1 - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.EnableDeviceData(); - /// MyClient.GetFrame(); - /// Output_GetDeviceOutputName Output = - /// MyClient.GetDeviceOutputName( "AMTI", 0 ); - /// // Output.Result == Success - /// // Output.DeviceOutputName == "Fx" - /// // Output.DeviceOutputUnit == Newton - /// - /// MATLAB example - /// - /// A valid Device Output Index is between 1 and GetDeviceOutputCount() - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.EnableDeviceData(); - /// MyClient.GetFrame(); - /// Output = MyClient.GetDeviceOutputName( "AMTI", 0 ); - /// % Output.Result == Success - /// % Output.DeviceOutputName == "Fx" - /// % Output.DeviceOutputUnit == Newton - /// - /// .NET example - /// - /// A valid Device Output Index is between 0 and GetDeviceOutputCount()-1 - /// ViconDataStreamSDK.DotNET.Client MyClient = - /// new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.EnableDeviceData(); - /// MyClient.GetFrame(); - /// Output_GetDeviceOutputName Output = - /// MyClient.GetDeviceOutputName( "AMTI", 0 ); - /// // Output.Result == Success - /// // Output.DeviceOutputName == "Fx" - /// // Output.DeviceOutputUnit == Newton - /// ----- - /// \param DeviceName The device name - /// \param DeviceOutputIndex The index of the device output - /// \return An Output_GetDeviceOutputName class containing the result of the operation, the name of the device output and the unit of the device output. - /// - The Result will be: - /// + Success - /// + NotConnected - /// - /// - The DeviceOutputName could be: - /// + "Fx" - Force X - /// + "Fy" - Force Y - /// + "Fz" - Force Z - /// + "Mx" - Moment X - /// + "My" - Moment Y - /// + "Mz" - Moment Z - /// + "Cx" - Center Of Pressure X - /// + "Cy" - Center Of Pressure Y - /// + "Cz" - Center Of Pressure Z - /// + "Pin1" - Analog Input 1 - /// + "Pin2" - Analog Input 2 - // - /// - The Device Output Unit will be: - /// + Unit.Unknown - /// + Unit.Volt - /// + Unit.Newton - /// + Unit.NewtonMeter - /// + Unit.Meter - /// + Unit.Kilogram - /// + Unit.Second - /// + Unit.Ampere - /// + Unit.Kelvin - /// + Unit.Mole - /// + Unit.Candela - /// + Unit.Radian - /// + Unit.Steradian - /// + Unit.MeterSquared - /// + Unit.MeterCubed - /// + Unit.MeterPerSecond - /// + Unit.MeterPerSecondSquared - /// + Unit.RadianPerSecond - /// + Unit.RadianPerSecondSquared - /// + Unit.Hertz - /// + Unit.Joule - /// + Unit.Watt - /// + Unit.Pascal - /// + Unit.Lumen - /// + Unit.Lux - /// + Unit.Coulomb - /// + Unit.Ohm - /// + Unit.Farad - /// + Unit.Weber - /// + Unit.Tesla - /// + Unit.Henry - /// + Unit.Siemens - /// + Unit.Becquerel - /// + Unit.Gray - /// + Unit.Sievert - /// + Unit.Katal - Output_GetDeviceOutputName GetDeviceOutputName( const String & DeviceName, - const unsigned int DeviceOutputIndex ) const; - - /// Return the value of a device output. If there are multiple samples for a frame, then the first sample is returned. - /// The force plate data provided in the individual device channels is in a coordinate system local to the force plate aligned Z upwards, Y towards the front of the force plate. - /// This coordinate system is located at the center of the top surface of the force plate. Any plate origin offset has been accounted for in the moment data. These are forces not reactions. - /// - /// See Also: GetDeviceCount(), GetDeviceOutputCount(), GetDeviceOutputName() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableDeviceData( pClient ); - /// Client_GetFrame( pClient ); - /// COutput_GetDeviceOutputValue _Output_GetDeviceOutputValue; - /// Client_GetDeviceOutputValue( pClient, "AMTI", "Fx", &_Output_GetDeviceOutputValue ); - /// // _OutputGetDeviceOutputValue.Result == Success - /// // _OutputGetDeviceOutputValue.Value == ? - /// // _OutputGetDeviceOutputValue.Value.Occluded = ? - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableDeviceData(); - /// MyClient.GetFrame(); - /// Output_GetDeviceOutputValue Output = - /// MyClient.GetDeviceOutputValue( "AMTI", "Fx" ); - /// // Output.Result == Success - /// // Output.Value == ? - /// // Output.Occluded = ? - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableDeviceData(); - /// MyClient.GetFrame(); - /// Output = MyClient.GetDeviceOutputValue( "AMTI", "Fx" ); - /// // Output.Result == Success - /// // Output.Value == ? - /// // Output.Occluded = ? - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = - /// new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableDeviceData(); - /// MyClient.GetFrame(); - /// Output_GetDeviceOutputValue Output = - /// MyClient.GetDeviceOutputValue( "AMTI", "Fx" ); - /// // Output.Result == Success - /// // Output.Value == ? - /// // Output.Occluded = ? - /// ----- - /// \param DeviceName The device name - /// \param DeviceOutputName The name of the device output - /// \return An Output_GetDeviceOutputValue class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidDeviceName - /// + InvalidDeviceOutputName - Output_GetDeviceOutputValue GetDeviceOutputValue( const String & DeviceName, - const String & DeviceOutputName ) const; - - /// Return the number of samples available for the specified device at the current frame. If an analog device is sampling at 1000 Hz and the system is running at 100 Hz then this function will return 10. - /// The samples can be accessed by supplying the subsample index to GetDeviceOutputValue. See below. - /// - /// See Also:GetDeviceOutputCount(), GetDeviceOutputValue() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableDeviceData( pClient ); - /// Client_GetFrame( pClient ); - /// COutput_GetDeviceOutputSubsamples DeviceOutputSubsamples; - /// Client_GetDeviceOutputSubsamples( pClient, "AMTI", "Fx", &DeviceOutputSubsamples ); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableDeviceData(); - /// MyClient.GetFrame(); - /// Output_GetDeviceOutputSubsamples Output = - /// MyClient.GetDeviceOutputSubsamples ( "AMTI", "Fx" ); - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableDeviceData(); - /// MyClient.GetFrame(); - /// Output = MyClient.GetDeviceOutputSubsamples ( "AMTI", "Fx" ); - /// // Output.Result == Success - /// // Output.DeviceOutputSubsamples == ? - /// // Output.Occluded = ? - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = - /// new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableDeviceData(); - /// MyClient.GetFrame(); - /// Output_GetDeviceOutputSubsamples Output = - /// MyClient.GetDeviceOutputSubsamples( "AMTI", "Fx" ); - /// // Output.Result == Success - /// // Output.DeviceOutputSubsamples == ? - /// // Output.Occluded = ? - /// ----- - /// \param DeviceName The device name - /// \param DeviceOutputName The name of the device output - /// \return An Output_GetDeviceOutputSubsamples class containing the result of the operation, the number of subsamples for this device output, and whether the device is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidDeviceName - /// + InvalidDeviceOutputName - /// - Occluded will be true if the value was absent at this frame. In this case the value will be 0. - Output_GetDeviceOutputSubsamples GetDeviceOutputSubsamples( const String & DeviceName, - const String & DeviceOutputName ) const; - /// Return the value of a device output. This override allows access to the individual subsamples for the current frame of data. - /// See GetDeviceOutputValue for information about the meaning of the force plate channels. - /// - /// See Also: GetDeviceOutputSubsamples(), GetDeviceOutputValue() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableDeviceData( pClient ); - /// Client_GetFrame( pClient ); - /// COutput_GetDeviceOutputValue _Output_GetDeviceOutputValue; - /// Client_GetDeviceOutputValueForSubsample( pClient, "AMTI", "Fx", 6, &_Output_GetDeviceOutputValue); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableDeviceData(); - /// MyClient.GetFrame(); - /// Output_GetDeviceOutputValue Output = - /// MyClient.GetDeviceOutputValue( "AMTI", "Fx", 6 ); - /// // Output.Result == Success - /// // Output.Value == ? - /// // Output.Occluded = ? - /// - /// MATLAB example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableDeviceData(); - /// MyClient.GetFrame(); - /// Output_GetDeviceOutputValue Output = - /// MyClient.GetDeviceOutputValue( "AMTI", "Fx", 6 ); - /// // Output.Result == Success - /// // Output.Value == ? - /// // Output.Occluded = ? - /// // Output.Value == ? - /// // Output.Occluded = ? - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableDeviceData(); - /// MyClient.GetFrame(); - /// Output_GetDeviceOutputValue Output = - /// MyClient.GetDeviceOutputValue( "AMTI", "Fx", 6 ); - /// // Output.Result == Success - /// // Output.Value == ? - /// // Output.Occluded = ? - /// ----- - /// \param DeviceName The device name - /// \param DeviceOutputName The name of the device output - /// \param Subsample The subsamples to access - /// \return An Output_GetDeviceOutputValue class containing the result of the operation, the value of the device output, and whether the device is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidDeviceName - /// + InvalidDeviceIndex - /// + InvalidDeviceOutputName - Output_GetDeviceOutputValue GetDeviceOutputValue( const String & DeviceName, - const String & DeviceOutputName, - const unsigned int Subsample ) const; - - /// Return the number of force plates available in the DataStream. - /// - /// See Also: GetGlobalForceVector(), GetGlobalMomentVector(), GetGlobalCentreOfPressure() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_EnableDeviceData( pClient ); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// COutput_GetForcePlateCount ForcePlateCount; - /// Client_GetForcePlateCount(pClient, &ForcePlateCount); - /// // ForcePlateCount.Result == Success - /// // ForcePlateCount.ForcePlateCount >= 0 - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.EnableDeviceData(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetForcePlateCount Output = MyClient.GetForcePlateCount (); - /// // Output.Result == Success - /// // Output.ForcePlateCount >= 0 - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.EnableDeviceData(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetForcePlateCount(); // Output.Result == Success - /// // Output.ForcePlateCount >= 0 - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.EnableDeviceData(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetForcePlateCount Output = MyClient.GetForcePlateCount(); - /// // Output.Result == Success - /// // Output.ForcePlateCount >= 0 - /// ----- - /// \return An Output_GetForcePlateCount class containing the result of the operation and the number of force plates. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - Output_GetForcePlateCount GetForcePlateCount() const; - - /// Return the force vector for the force plate in global coordinates. - /// The vector is in Newtons and is with respect to the global coordinate system regardless of the orientation of the force plate. The vector represents the force exerted upon the force plate, not the reaction force. - /// If multiple subsamples are available this function returns the first subsample. See the alternate version of this function to access all of the analog data. - /// - /// See Also: GetGlobalMomentVector(), GetGlobalCentreOfPressure() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableDeviceData ( pClient ); - /// Client_GetFrame( pClient ); - /// COutput_GetGlobalForceVector _Output_GetForceVector; - /// Client_GetGlobalForceVector( pClient, 0, &_Output_GetForceVector); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// A valid ForcePlateIndex is between 0 and GetForcePlateCount()-1 - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableDeviceData (); - /// MyClient.GetFrame(); - /// Output_GetGlobalForceVector Output = MyClient.GetGlobalForceVector( 0 ); - /// - /// MATLAB example - /// - /// A valid ForcePlateIndex is between 1 and GetForcePlateCount() - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableDeviceData (); - /// MyClient.GetFrame(); - /// Output = MyClient.GetGlobalForceVector( 1 ); - /// - /// .NET example - /// - /// A valid ForcePlateIndex is between 0 and GetForcePlateCount() - 1 - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableUnlabeledMarkerData(); - /// MyClient.GetFrame(); - /// Output_GetGlobalForceVector Output = MyClient.GetGlobalForceVector( 0 ); - /// ----- - /// \param ForcePlateIndex The index of the force plate - /// \return An Output_GetGlobalForceVector class containing the result of the operation and the force on the force plate - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - Output_GetGlobalForceVector GetGlobalForceVector( const unsigned int ForcePlateIndex ) const; - - /// Return the moment vector for the force plate in global coordinates. - /// The vector is in Newton-meters and is with respect to the global coordinate system regardless of the orientation of the force plate. - /// The vector represents the moment exerted upon the force plate, not the reaction moment. Any force plate origin offset is accounted for in the moments so they are acting about the exact center of the top surface of the force plate. - /// If multiple subsamples are available this function returns the first subsample. See the alternate version of this function to access all of the analog data. - /// - /// See Also: GetGlobalForceVector(), GetGlobalCentreOfPressure() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableDeviceData ( pClient ); - /// Client_GetFrame( pClient ); - /// COutput_GetGlobalMomentVector _Output_GetMomentVector; - /// Client_GetGlobalMomentVector( pClient, 0, &_Output_GetMomentVector ); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// A valid ForcePlateIndex is between 0 and GetForcePlateCount()-1 - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableDeviceData (); - /// MyClient.GetFrame(); - /// Output_GetGlobalMomentVector Output = MyClient.GetGlobalMomentVector( 0 ); - /// - /// MATLAB example - /// - /// A valid ForcePlateIndex is between 1 and GetForcePlateCount() - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableDeviceData (); - /// MyClient.GetFrame(); - /// Output = MyClient.GetGlobalMomentVector( 1 ); - /// - /// .NET example - /// - /// A valid ForcePlateIndex is between 0 and GetForcePlateCount() - 1 - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableDeviceData (); - /// MyClient.GetFrame(); - /// Output_GetGlobalMomentVector Output = MyClient.GetGlobalMomentVector( 0 ); - /// ----- - /// \param ForcePlateIndex The index of the force plate - /// \return An Output_GetGlobalMomentVector class containing the result of the operation and the moment exerted on the force plate - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - Output_GetGlobalMomentVector GetGlobalMomentVector( const unsigned int ForcePlateIndex ) const; - - /// Return the center of pressure for the force plate in global coordinates. - /// The position is in millimeters and is with respect to the global coordinate system. - /// If multiple subsamples are available this function returns the first subsample. See the alternate version of this function to access all of the analog data. - /// - /// See Also: GetGlobalForceVector(), GetGlobalMomentVector() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_EnableDeviceData ( pClient ); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// COutput_GetGlobalCentreOfPressure _Output_GetCentreOfPressure; - /// Client_GetGlobalCentreOfPressure( pClient, 0, &_Output_GetCentreOfPressure ); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// A valid ForcePlateIndex is between 0 and GetForcePlateCount()-1 - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableDeviceData (); - /// MyClient.GetFrame(); - /// Output_GetGlobalCentreOfPressure Output = MyClient.GetGlobalCentreOfPressure( 0 ); - /// - /// MATLAB example - /// - /// A valid ForcePlateIndex is between 1 and GetForcePlateCount() - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableDeviceData (); - /// MyClient.GetFrame(); - /// Output = MyClient.GetGlobalCentreOfPressure( 1 ); - /// - /// .NET example - /// - /// A valid ForcePlateIndex is between 0 and GetForcePlateCount() - 1 - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableDeviceData (); - /// MyClient.GetFrame(); - /// Output_GetGlobalCentreOfPressure Output = MyClient.GetGlobalCentreOfPressure( 0 ); - /// ----- - /// \param ForcePlateIndex The index of the force plate - /// \return An Output_GetGlobalCentreOfPressure class containing the result of the operation and the center of pressure. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - Output_GetGlobalCentreOfPressure GetGlobalCentreOfPressure( const unsigned int ForcePlateIndex ) const; - - /// Return the number of subsamples available for a specified plate in the current frame. - /// Additional versions of GetGlobalForceVector, GetGlobalMomentVector, and GetGlobalCentreOfPressure take the subsample index to allow access to all the force plate data. - /// - /// See Also: GetGlobalForceVector(), GetGlobalMomentVector(), GetGlobalCentreOfPressure() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableDeviceData ( pClient ); - /// Client_GetFrame( pClient ); - /// COutput_GetForcePlateSubsamples ForcePlateSubsamples; - /// Client_GetForcePlateSubsamples( pClient, 0, &ForcePlateSubsamples ); - /// // ForcePlateSubsamples.Result == Success - /// // ForcePlateSubsamples.ForcePlateSubsamples >= 0 - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// A valid ForcePlateIndex is between 0 and GetForcePlateCount()-1 - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.EnableDeviceData(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetForcePlateSubsamples Output = MyClient.GetForcePlateSubsamples ( 0 ); - /// // Output.Result == Success - /// // Output.ForcePlateSubsamples >= 0 - - /// - /// MATLAB example - /// - /// A valid ForcePlateIndex is between 1 and GetForcePlateCount() - /// MyClient = Client(); - /// MyClient.EnableDeviceData(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetForcePlateSubsamples( 1 ); - /// // Output.Result == Success - /// // Output.ForcePlateSubsamples >= 0 - /// - /// .NET example - /// - /// A valid ForcePlateIndex is between 0 and GetForcePlateCount()-1 - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.EnableDeviceData(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetForcePlateSubsamples Output = MyClient.GetForcePlateSubsamples ( 0 ); - /// // Output.Result == Success - /// // Output.ForcePlateSubsamples >= 0 - /// ----- - - /// - /// \param ForcePlateIndex The index of the force plate - /// \return An Output_GetForcePlateSubsamples class containing the result of the operation and the number of subsamples. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - Output_GetForcePlateSubsamples GetForcePlateSubsamples( const unsigned int ForcePlateIndex ) const; - - /// Return the force vector for the plate in global coordinates. This version takes a subsample index that allows access to all of the force information. - /// The vector is in Newtons and is with respect to the global coordinate system regardless of the orientation of the plate. The vector represents the force exerted upon the plate, not the reaction force. - /// - /// See Also: GetGlobalMomentVector(), GetGlobalCentreOfPressure() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableDeviceData ( pClient ); - /// Client_GetFrame( pClient ); - /// unsigned int Index(0); - /// COutput_GetForcePlateSubsamples ForcePlateSubsamples; - /// Client_GetForcePlateSubsamples( pClient, Index, &ForcePlateSubsamples ); - /// for ( unsigned int ForcePlateSubsample = 0; - /// ForcePlateSubsample < ForcePlateSubsamples.ForcePlateSubsamples; ++ForcePlateSubsample) - /// { - /// COutput_GetGlobalForceVector _Output_GetForceVector; - /// Client_GetGlobalForceVectorForSubsample( - /// pClient, Index, ForcePlateSubsample, &_Output_GetForceVector ); - /// } - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// A valid ForcePlateIndex is between 0 and GetForcePlateCount()-1 - /// A valid Subsample is between 0 and GetForcePlateSubsamples()-1 - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableDeviceData (); - /// MyClient.GetFrame(); - /// const unsigned int Index(0); - /// const unsigned int Samples = MyClient.GetForcePlateSubsamples( index ).ForcePlateSubsamples; - /// for( unsigned int Sample = 0; Sample < Samples; ++ Sample) - /// { - /// Output_GetGlobalForceVector Output = MyClient.GetGlobalForceVector( Index, Sample ); - /// } - /// - /// MATLAB example - /// - /// A valid ForcePlateIndex is between 1 and GetForcePlateCount() - /// A valid Subsample is between 1 and GetForcePlateSubsamples() - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableDeviceData (); - /// MyClient.GetFrame(); - /// Index = 0; - /// Output_GetForcePlateSubsamples = MyClient.GetForcePlateSubsamples(Index ); - /// for Sample = 1:Output_GetForcePlateSubsamples.ForcePlateSubsamples - /// Output = MyClient.GetGlobalForceVector( Index, Sample ); - /// end - /// - /// .NET example - /// - /// A valid ForcePlateIndex is between 0 and GetForcePlateCount()-1 - /// A valid Subsample is between 0 and GetForcePlateSubsamples()-1 - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableUnlabeledMarkerData(); - /// MyClient.GetFrame(); - /// uint Index = 0; - /// uint Samples = MyClient.GetForcePlateSubsamples(ForcePlateIndex).ForcePlateSubsamples; - /// for (uint Sample = 0; Sample < Samples; ++ Sample) - /// { - /// Output_GetGlobalForceVector Output = MyClient.GetGlobalForceVector( Index, Sample ); - /// } - /// ----- - /// \param ForcePlateIndex The index of the force plate - /// \param Subsample The subsample to access - /// \return An Output_GetGlobalForceVector class containing the result of the operation and the force on the plate. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - Output_GetGlobalForceVector GetGlobalForceVector( const unsigned int ForcePlateIndex, const unsigned int Subsample ) const; - - /// Return the moment vector for the plate in global coordinates. This version takes a subsample index that allows access to all of the force information. - /// The vector is in Newton-meters and is with respect to the global coordinate system regardless of the orientation of the plate. - /// The vector represents the moment exerted upon the plate, not the reaction moment. Any force plate origin offset is accounted for in the moments so they are acting about the exact center of the top surface of the plate. - /// - /// See Also: GetGlobalForceVector(), GetGlobalCentreOfPressure() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_EnableDeviceData ( pClient ); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// unsigned int Index(0); - /// COutput_GetForcePlateSubsamples ForcePlateSubsamples; - /// Client_GetForcePlateSubsamples( pClient, Index, &ForcePlateSubsamples ); - /// for ( unsigned int ForcePlateSubsample = 0; - /// ForcePlateSubsample < ForcePlateSubsamples.ForcePlateSubsamples; ++ForcePlateSubsample) - /// { - /// COutput_GetGlobalMomentVector _Output_GetMomentVector; - /// Client_GetGlobalMomentVectorForSubsample( - /// pClient, Index, ForcePlateSubsample, &_Output_GetMomentVector); - /// } - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// A valid ForcePlateIndex is between 0 and GetForcePlateCount()-1 - /// A valid Subsample is between 0 and GetForcePlateSubsamples()-1 - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableDeviceData (); - /// MyClient.GetFrame(); - /// const unsigned int Index(0); - /// const unsigned int Samples = MyClient.GetForcePlateSubsamples( index ).ForcePlateSubsamples; - /// for( unsigned int Sample = 0; Sample < Samples; ++ Sample) - /// { - /// Output_GetGlobalMomentVector Output = MyClient.GetGlobalMomentVector( Index, Sample ); - /// } - /// - /// MATLAB example - /// - /// A valid ForcePlateIndex is between 1 and GetForcePlateCount() - /// A valid Subsample is between 1 and GetForcePlateSubsamples() - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableDeviceData (); - /// MyClient.GetFrame(); - /// Index = 0; - /// Output_GetForcePlateSubsamples = MyClient.GetForcePlateSubsamples( Index ); - /// for Sample = 1:Output_GetForcePlateSubsamples.ForcePlateSubsamples - /// Output = MyClient.GetGlobalMomentVector ( Index, Sample ); - /// end - /// - /// .NET example - /// - /// A valid ForcePlateIndex is between 0 and GetForcePlateCount() - 1 - /// A valid Subsample is between 0 and GetForcePlateSubsamples()-1 - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableDeviceData (); - /// MyClient.GetFrame(); - /// uint Index = 0; - /// uint Samples = MyClient.GetForcePlateSubsamples(ForcePlateIndex).ForcePlateSubsamples; - /// for (uint Sample = 0; Sample < Samples; ++ Sample) - /// { - /// Output_GetGlobalMomentVector Output = MyClient.GetGlobalMomentVector( Index, Sample ); - /// } - /// ----- - /// - /// - /// - /// \param ForcePlateIndex The index of the force plate - /// \param Subsample The subsample to access - /// \return An Output_GetGlobalMomentVector class containing the result of the operation and the moment exerted on the plate. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - Output_GetGlobalMomentVector GetGlobalMomentVector( const unsigned int ForcePlateIndex, const unsigned int Subsample ) const; - - /// Return the center of pressure for the plate in global coordinates. This version takes a subsample index that allows access to all of the force information. - /// The position is in millimeters and is with respect to the global coordinate system. - /// - /// See Also: GetGlobalForceVector(), GetGlobalMomentVector() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableDeviceData ( pClient ); - /// Client_GetFrame( pClient ); - /// unsigned int Index(0); - /// COutput_GetForcePlateSubsamples ForcePlateSubsamples; - /// Client_GetForcePlateSubsamples( pClient, Index, &ForcePlateSubsamples ); - /// for ( unsigned int ForcePlateSubsample = 0; - /// ForcePlateSubsample < ForcePlateSubsamples.ForcePlateSubsamples; ++ForcePlateSubsample) - /// { - /// COutput_GetGlobalCentreOfPressure _Output_GetCentreOfPressure; - /// Client_GetGlobalCentreOfPressureForSubsample( - /// pClient, Index, ForcePlateSubsample, &_Output_GetCentreOfPressure); - /// } - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// A valid ForcePlateIndex is between 0 and GetForcePlateCount()-1 - /// A valid Subsample is between 0 and GetForcePlateSubsamples()-1 - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableDeviceData (); - /// MyClient.GetFrame(); - /// const unsigned int Index(0); - /// const unsigned int Samples = MyClient.GetForcePlateSubsamples( index ).ForcePlateSubsamples; - /// for( unsigned int Sample = 0; Sample < Samples; ++ Sample) - /// { - /// Output_GetGlobalCentreOfPressure Output = MyClient.GetGlobalCentreOfPressure(Index,Sample); - /// } - /// - /// MATLAB example - /// - /// A valid ForcePlateIndex is between 1 and GetForcePlateCount() - /// A valid Subsample is between 1 and GetForcePlateSubsamples() - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// Client_GetFrame( pClient ); - /// MyClient.EnableDeviceData (); - /// MyClient.GetFrame(); - /// Index = 0; - /// Output_GetForcePlateSubsamples = MyClient.GetForcePlateSubsamples( Index ); - /// for Sample = 1:Output_GetForcePlateSubsamples.ForcePlateSubsamples - /// Output = MyClient.GetGlobalCentreOfPressure( Index, Sample ); - /// end - /// - /// .NET example - /// - /// A valid ForcePlateIndex is between 0 and GetForcePlateCount()-1 - /// A valid Subsample is between 0 and GetForcePlateSubsamples()-1 - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableDeviceData (); - /// MyClient.GetFrame(); - /// uint Index = 0; - /// uint Samples = MyClient.GetForcePlateSubsamples(ForcePlateIndex).ForcePlateSubsamples; - /// for (uint Sample = 0; Sample < Samples; ++ Sample) - /// { - /// Output_GetGlobalCentreOfPressure Output = MyClient.GetGlobalCentreOfPressure (Index,Sample); - /// } - /// ----- - /// \param ForcePlateIndex The index of the force plate - /// \param Subsample The subsample to access - /// \return An Output_GetGlobalCentreOfPressure class containing the result of the operation the center of pressure - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - Output_GetGlobalCentreOfPressure GetGlobalCentreOfPressure( const unsigned int ForcePlateIndex, const unsigned int Subsample ) const; - - /// Return the number of eye trackers available in the DataStream. - /// - /// See Also: GetEyeTrackerGlobalGazeVector(), GetEyeTrackerGlobalGazeVector() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_EnableDeviceData( pClient ); - /// Client_Connect( pClient, "localhost" ); - /// Client_GetFrame( pClient ); - /// COutput_GetEyeTrackerCount EyeTrackerCount; - /// Client_GetEyeTrackerCount(pClient, &EyeTrackerCount); - /// // EyeTrackerCount.Result == Success - /// // EyeTrackerCount.EyeTrackerCount >= 0 - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.EnableDeviceData(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetDeviceCount Output = MyClient.GetEyeTrackerCount (); - /// // Output.Result == Success - /// // Output.EyeTrackerCount >= 0 - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.EnableDeviceData(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetEyeTrackerCount(); - /// // Output.Result == Success - /// // Output.EyeTrackerCount >= 0 - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.EnableDeviceData(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetEyeTrackerCount Output = MyClient.GetEyeTrackerCount(); - /// // Output.Result == Success - /// // Output.EyeTrackerCount >= 0 - /// ----- - /// \return An Output_GetEyeTrackerCount class containing the result of the operation and the number of eye trackers. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - Output_GetEyeTrackerCount GetEyeTrackerCount() const; - - /// Return the location of the eye. The position is in millimeters with respect to the global origin. The segment and device data need to be enabled to get the position. - /// - /// See Also: GetEyeTrackerCount(), GetEyeTrackerGlobalGazeVector() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableDeviceData( pClient ); - /// Client_GetFrame( pClient ); - /// COutput_GetEyeTrackerGlobalPosition _Output_GetEyeTrackerGlobalPosition; - /// Client_GetEyeTrackerGlobalPosition(pClient, 0, &_Output_GetEyeTrackerGlobalPosition); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// A valid EyeTrackerIndex is between 0 and GetEyeTrackerCount()-1 - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData (); - /// MyClient.EnableDeviceData (); - /// MyClient.GetFrame(); - /// Output_GetEyeTrackerGlobalPosition Output = MyClient.GetEyeTrackerGlobalPosition ( 0 ); - /// - /// MATLAB example - /// - /// A valid EyeTrackerIndex is between 1 and GetEyeTrackerCount() - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData (); - /// MyClient.EnableDeviceData (); - /// MyClient.GetFrame(); - /// Output = MyClient.GetEyeTrackerGlobalPosition ( 1 ); - /// - /// .NET example - /// - /// A valid EyeTrackerIndex is between 0 and GetEyeTrackerCount() - 1 - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData (); - /// MyClient.EnableDeviceData (); - /// MyClient.GetFrame(); - /// Output_GetEyeTrackerGlobalPosition Output = MyClient.GetEyeTrackerGlobalPosition ( 0 ); - /// ----- - /// \param EyeTrackerIndex The index of the eye tracker - /// \return An Output_GetEyeTrackerGlobalPosition class containing the result of the operation, the eye position and whether the eyetracker is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - /// - Occluded will be true if the segment that has the eye tracker attached is not visible. If true the position will be (0,0,0). - Output_GetEyeTrackerGlobalPosition GetEyeTrackerGlobalPosition( const unsigned int EyeTrackerIndex ) const; - - /// Return the gaze direction as a unit vector in global coordinates. - /// The gaze vector will be marked as occluded if the segment that has the eye tracker attached is not visible, the eye tracker is not calibrated or the pupil is not found. - /// The segment and device data need to be enabled to get the gaze vector. - /// - /// See Also: GetEyeTrackerCount(), GetEyeTrackerGlobalPosition() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableDeviceData( pClient ); - /// Client_GetFrame( pClient ); - /// COutput_GetEyeTrackerGlobalGazeVector _Output_GetEyeTrackerGlobalGazeVector; - /// Client_GetEyeTrackerGlobalGazeVector(pClient, 0, &_Output_GetEyeTrackerGlobalGazeVector); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// A valid EyeTrackerIndex is between 0 and GetEyeTrackerCount()-1 - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData (); - /// MyClient.EnableDeviceData (); - /// MyClient.GetFrame(); - /// Output_GetEyeTrackerGlobalPosition Output = MyClient.GetEyeTrackerGlobalGazeVector ( 0 ); - /// - /// MATLAB example - /// - /// A valid EyeTrackerIndex is between 1 and GetEyeTrackerCount() - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData (); - /// MyClient.EnableDeviceData (); - /// MyClient.GetFrame(); - /// Output = MyClient.GetEyeTrackerGlobalGazeVector ( 1 ); - /// - /// .NET example - /// - /// A valid EyeTrackerIndex is between 0 and GetEyeTrackerCount() - 1 - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData (); - /// MyClient.EnableDeviceData (); - /// MyClient.GetFrame(); - /// Output_GetEyeTrackerGlobalPosition Output = MyClient.GetEyeTrackerGlobalPosition ( 0 ); - /// ----- - /// \param EyeTrackerIndex The index of the eye tracker - /// \return An Output_GetEyeTrackerGlobalGazeVector class containing the result of the operation, the gaze direction vector, and whether the eye tracker is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - /// - Occluded will be true if the gaze vector could not be calculated. If true, the position will be (0,0,0). - Output_GetEyeTrackerGlobalGazeVector GetEyeTrackerGlobalGazeVector( const unsigned int EyeTrackerIndex ) const; - - /// Return the number of cameras available in the DataStream. - /// - /// See Also: GetCameraName(), GetCentroidCount(), GetCentroidPosition() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableCentroidData( pClient ); - /// COutput_GetCameraCount CameraCount; - /// Client_GetCameraCount(pClient, &CameraCount); - /// // CameraCount.Result == Success - /// // CameraCount.CameraCount >= 0 - /// Client_GetFrame( pClient ); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableCentroidData (); - /// MyClient.GetFrame(); - /// Output_GetCameraCount Output = MyClient.GetCameraCount(); - /// // Output.Result == Success - /// // Output.CameraCount >= 0 - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableCentroidData(); - /// MyClient.GetFrame(); - /// Output = MyClient.GetCameraCount(); - /// % Output.Result == Success, Output.CameraCount >= 0 - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableCentroidData (); - /// MyClient.GetFrame(); - /// Output_GetCameraCount Output = MyClient.GetCameraCount(); - /// // Output.Result == Success - /// // Output.CameraCount >= 0 - /// ----- - /// \return An Output_GetCameraCount class containing the result of the operation and the number of cameras. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - Output_GetCameraCount GetCameraCount() const; - - /// Return the name of a camera. This name can be passed into centroid functions. - /// - /// See Also: GetCameraCount(), GetCentroidCount(), GetCentroidPosition() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableCentroidData ( pClient ); - /// Client_GetFrame( pClient ); - /// COutput_GetCameraCount CameraCount; - /// Client_GetCameraCount(pClient, &CameraCount); - /// // CamerCount.Result == Success - /// // CameraCount.CameraCount == 1 - /// char CameraName[128]; - /// Client_GetCameraName(pClient, 0, 128, CameraName); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// A valid CameraIndex is between 0 and GetCameraCount()-1 - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableCentroidData (); - /// MyClient.GetFrame(); - /// Output_GetCameraCount OutputGCC = MyClient.GetCameraCount(); - /// // OutputGCC.Result == Success - /// // OutputGCC.CameraCount == 1 - /// Output_GetCameraName OutputGCN; - /// OutputGCN = MyClient.GetCameraName( 0 ) - /// - /// MATLAB example - /// - /// A valid CameraIndex is between 1 and GetCameraCount() - /// % [Output] = GetCameraName ( CameraIndex ) - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableCentroidData (); - /// MyClient.GetFrame(); - /// OutputGCC = MyClient.GetCameraCount ( 1 ); - /// % OutputGCC.Result == Success - /// % OutputGCC.CameraCount == 1 - /// OutputGCN = MyClient.GetCameraName( 1 ); - /// - /// .NET example - /// - /// A valid CameraIndex is between 0 and GetCameraCount() - 1 - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableCentroidData (); - /// MyClient.GetFrame(); - /// Output_GetCameraCount OutputGCC = MyClient.GetCameraCount(); - /// // OutputGCC.Result == Success - /// // OutputGCC.CameraCount == 1 - /// Output_GetCameraName OutputGCN; - /// OutputGCN = MyClient.GetCameraName( 0 ) - /// ----- - /// \param CameraIndex The index of the camera - /// \return An Output_GetCameraName class containing the result of the operation and the name of the camera. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - Output_GetCameraName GetCameraName( unsigned int CameraIndex ) const; - - /// Returns the internal ID of the camera with the specified name. - /// - /// See Also: GetCameraName() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableCentroidData ( pClient ); - /// COutput_GetCameraCount CameraCount; - /// Client_GetCameraCount(pClient, &CameraCount); - /// if( CameraCount.Result == Success && CameraCount.CameraCount > 0 ) - /// { - /// char CameraName[128]; - /// CEnum Output = Client_GetCameraName(pClient, 0, 128, CameraName); - /// if ( Output == Success ) - /// { - /// COutput_GetCameraId CameraId; - /// Client_GetCameraId(pClient, CameraName, &CameraId ); - /// } - /// } - /// Client_GetFrame( pClient ); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// A valid CameraName may be obtained from GetCameraName() - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableCentroidData (); - /// MyClient.GetFrame(); - /// Output_GetCameraCount OutputGCC = MyClient.GetCameraCount(); - /// if( OutputGCC.Result == Success && OutputGCC.CameraCount > 0 ) - /// { - /// Output_GetCameraName OutputGCN; - /// OutputGCN = MyClient.GetCameraName( 0 ); - /// if( OutputGCN.Result == Success ) - /// { - /// Output_GetCameraId Output_GCI = MyClient.GetCameraId( OutputGCN.CameraName ); - /// } - /// } - /// - /// MATLAB example - /// - /// Not implemented - /// - /// - /// .NET example - /// - /// A valid CameraName may be obtained from GetCameraName() - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableCentroidData (); - /// MyClient.GetFrame(); - /// Output_GetCameraCount OutputGCC = MyClient.GetCameraCount(); - /// if( OutputGCC.Result == Success && OutputGCC.CameraCount > 0 ) - /// { - /// Output_GetCameraName OutputGCN; - /// OutputGCN = MyClient.GetCameraName( 0 ); - /// if( OutputGCN.Result == Success ) - /// { - /// Output_GetCameraId Output_GCI = MyClient.GetCameraId( OutputGCN.CameraName ); - /// } - /// } - /// ----- - /// - /// \return An Output_GetCameraId class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + InvalidCameraName - Output_GetCameraId GetCameraId( const std::string & CameraName ) const; - - /// Returns the user-assigned ID of the camera with the specified name. - /// - /// See Also: GetCameraName() - /// - /// - /// C example - /// - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableCentroidData ( pClient ); - /// Client_GetFrame( pClient ); - /// COutput_GetCameraCount CameraCount; - /// Client_GetCameraCount(pClient, &CameraCount); - /// if( CameraCount.Result == Success && CameraCount.CameraCount > 0 ) - /// { - /// char CameraName[128]; - /// CEnum Output = Client_GetCameraName(pClient, 0, 128, CameraName); - /// if ( Output == Success ) - /// { - /// COutput_GetCameraUserId CameraId; - /// Client_GetCameraUserId(pClient, CameraName, &CameraUserId ); - /// } - /// } - /// Client_Destroy( pClient ); - /// - /// - /// C++ example - /// - /// - /// A valid CameraName may be obtained from GetCameraName() - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableCentroidData (); - /// MyClient.GetFrame(); - /// Output_GetCameraCount OutputGCC = MyClient.GetCameraCount(); - /// if( OutputGCC.Result == Success && OutputGCC.CameraCount > 0 ) - /// { - /// Output_GetCameraName OutputGCN; - /// OutputGCN = MyClient.GetCameraName( 0 ); - /// if( OutputGCN.Result == Success ) - /// { - /// Output_GetCameraUserId Output_GCI = MyClient.GetCameraUserId( OutputGCN.CameraName ); - /// } - /// } - /// - /// - /// MATLAB example - /// - /// - /// Not implemented - /// - /// - /// - /// .NET example - /// - /// - /// A valid CameraName may be obtained from GetCameraName() - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableCentroidData (); - /// MyClient.GetFrame(); - /// Output_GetCameraCount OutputGCC = MyClient.GetCameraCount(); - /// if( OutputGCC.Result == Success && OutputGCC.CameraCount > 0 ) - /// { - /// Output_GetCameraName OutputGCN; - /// OutputGCN = MyClient.GetCameraName( 0 ); - /// if( OutputGCN.Result == Success ) - /// { - /// Output_GetCameraUserId Output_GCI = MyClient.GetCameraUserId( OutputGCN.CameraName ); - /// } - /// } - /// ----- - /// - /// \return An Output_GetCameraUserId class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + InvalidCameraName - Output_GetCameraUserId GetCameraUserId( const std::string & CameraName ) const; - - /// Returns the type of the camera with the specified name. - /// The type returned is an internal type string. - /// - /// See Also: GetCameraName() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableCentroidData ( pClient ); - /// Client_GetFrame( pClient ); - /// COutput_GetCameraCount CameraCount; - /// Client_GetCameraCount(pClient, &CameraCount); - /// if( CameraCount.Result == Success && CameraCount.CameraCount > 0 ) - /// { - /// char CameraName[128]; - /// CEnum Output = Client_GetCameraName(pClient, 0, 128, CameraName); - /// if ( Output == Success ) - /// { - /// char CameraType[128]; - /// CEnum Result = Client_GetCameraType( pClient, CameraName, 128, CameraType ); - /// } - /// } - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// A valid CameraName may be obtained from GetCameraName() - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableCentroidData (); - /// MyClient.GetFrame(); - /// Output_GetCameraCount OutputGCC = MyClient.GetCameraCount(); - /// if( OutputGCC.Result == Success && OutputGCC.CameraCount > 0 ) - /// { - /// Output_GetCameraName OutputGCN; - /// OutputGCN = MyClient.GetCameraName( 0 ); - /// if( OutputGCN.Result == Success ) - /// { - /// Output_GetCameraType Output_GCT = MyClient.GetCameraType( OutputGCN.CameraName ); - /// } - /// } - /// - /// MATLAB example - /// - /// Not implemented - /// - /// - /// .NET example - /// - /// A valid CameraName may be obtained from GetCameraName() - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableCentroidData (); - /// MyClient.GetFrame(); - /// Output_GetCameraCount OutputGCC = MyClient.GetCameraCount(); - /// if( OutputGCC.Result == Success && OutputGCC.CameraCount > 0 ) - /// { - /// Output_GetCameraName OutputGCN; - /// OutputGCN = MyClient.GetCameraName( 0 ); - /// if( OutputGCN.Result == Success ) - /// { - /// Output_GetCameraType Output_GCT = MyClient.GetCameraType( OutputGCN.CameraName ); - /// } - /// } - /// ----- - /// - /// \return An Output_ class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + InvalidCameraName - Output_GetCameraType GetCameraType( const std::string & CameraName ) const; - - /// Returns the name of of the camera type as a string suitable for display to a user. - /// - /// See Also: GetCameraName() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableCentroidData ( pClient ); - /// Client_GetFrame( pClient ); - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableCentroidData ( pClient ); - /// Client_GetFrame( pClient ); - /// COutput_GetCameraCount CameraCount; - /// Client_GetCameraCount(pClient, &CameraCount); - /// if( CameraCount.Result == Success && CameraCount.CameraCount > 0 ) - /// { - /// char CameraName[128]; - /// CEnum Output = Client_GetCameraName(pClient, 0, 128, CameraName); - /// if ( Output == Success ) - /// { - /// char CameraDisplayName[128]; - /// CEnum Result = Client_GetCameraDisplayName( pClient, CameraName, 128, CameraDisplayName ); - /// } - /// } - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// A valid CameraName may be obtained from GetCameraName() - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableCentroidData (); - /// MyClient.GetFrame(); - /// Output_GetCameraCount OutputGCC = MyClient.GetCameraCount(); - /// if( Output_GCC.Result == Success && OutputGCC.CameraCount > 0 ) - /// { - /// Output_GetCameraName OutputGCN; - /// OutputGCN = MyClient.GetCameraName( 0 ); - /// if( OutputGCN.Result == Success ) - /// { - /// Output_GetCameraDisplayName Output_GCD = MyClient.GetCameraDisplayName( OutputGCN.CameraName ); - /// } - /// } - /// - /// MATLAB example - /// - /// Not implemented - /// - /// - /// .NET example - /// - /// A valid CameraName may be obtained from GetCameraName() - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableCentroidData (); - /// MyClient.GetFrame(); - /// Output_GetCameraCount OutputGCC = MyClient.GetCameraCount(); - /// if( Output_GCC.Result == Success && OutputGCC.CameraCount > 0 ) - /// { - /// Output_GetCameraName OutputGCN; - /// OutputGCN = MyClient.GetCameraName( 0 ); - /// if( OutputGCN.Result == Success ) - /// { - /// Output_GetCameraDisplayName Output_GCD = MyClient.GetCameraDisplayName( OutputGCN.CameraName ); - /// } - /// } - /// ----- - /// - /// \return An Output_ class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + InvalidCameraName - Output_GetCameraDisplayName GetCameraDisplayName( const std::string & CameraName ) const; - - /// Returns the sensor resolution of the camera with the specified name. - /// - /// See Also: GetCameraName() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableCentroidData ( pClient ); - /// Client_GetFrame( pClient ); - /// COutput_GetCameraCount CameraCount; - /// Client_GetCameraCount(pClient, &CameraCount); - /// if( CameraCount.Result == Success && CameraCount.CameraCount > 0 ) - /// { - /// char CameraName[128]; - /// CEnum Output = Client_GetCameraName(pClient, 0, 128, CameraName); - /// if ( Output == Success ) - /// { - /// COutput_GetCameraResolution CameraResolution; - /// Client_GetCameraResolution(pClient, CameraName, &CameraResolution ); - /// } - /// } - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// A valid CameraName may be obtained from GetCameraName() - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableCentroidData (); - /// MyClient.GetFrame(); - /// Output_GetCameraCount OutputGCC = MyClient.GetCameraCount(); - /// if( OutputGCC.Result == Success && OutputGCC.CameraCount > 0 ) - /// { - /// Output_GetCameraName OutputGCN; - /// OutputGCN = MyClient.GetCameraName( 0 ); - /// if( OutputGCN.Result == Success ) - /// { - /// Output_GetCameraResolution Output_GCR = MyClient.GetCameraResolution( OutputGCN.CameraName ); - /// } - /// } - /// - /// MATLAB example - /// - /// Not implemented - /// - /// - /// .NET example - /// - /// A valid CameraName may be obtained from GetCameraName() - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableCentroidData (); - /// MyClient.GetFrame(); - /// Output_GetCameraCount OutputGCC = MyClient.GetCameraCount(); - /// if( OutputGCC.Result == Success && OutputGCC.CameraCount > 0 ) - /// { - /// Output_GetCameraName OutputGCN; - /// OutputGCN = MyClient.GetCameraName( 0 ); - /// if( OutputGCN.Result == Success ) - /// { - /// Output_GetCameraResolution Output_GCR = MyClient.GetCameraResolution( OutputGCN.CameraName ); - /// } - /// } - /// ----- - /// - /// \return An Output_ class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + InvalidCameraName - Output_GetCameraResolution GetCameraResolution( const std::string & CameraName ) const; - - /// Returns whether the camera with the specified name is a video camera. - /// - /// See Also: GetCameraName() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableCentroidData ( pClient ); - /// Client_GetFrame( pClient ); - /// COutput_GetCameraCount CameraCount; - /// Client_GetCameraCount(pClient, &CameraCount); - /// if( CameraCount.Result == Success && CameraCount.CameraCount > 0 ) - /// { - /// char CameraName[128]; - /// CEnum Output = Client_GetCameraName(pClient, 0, 128, CameraName); - /// if ( Output == Success ) - /// { - /// COutput_GetIsVideoCamera IsVideoCamera; - /// Client_GetIsVideoCamera(pClient, CameraName, &IsVideoCamera ); - /// } - /// } - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// A valid CameraName may be obtained from GetCameraName() - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableCentroidData (); - /// MyClient.GetFrame(); - /// Output_GetCameraCount OutputGCC = MyClient.GetCameraCount(); - /// if( OutputGCC.Result == Success && OutputGCC.CameraCount > 0 ) - /// { - /// Output_GetCameraName OutputGCN; - /// OutputGCN = MyClient.GetCameraName( 0 ); - /// if( OutputGCN.Result == Success ) - /// { - /// Output_GetIsVideoCamera Output_GCV = MyClient.GetIsVideoCamera( OutputGCN.CameraName ); - /// } - /// } - /// - /// MATLAB example - /// - /// Not implemented - /// - /// - /// .NET example - /// - /// A valid CameraName may be obtained from GetCameraName() - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableCentroidData (); - /// MyClient.GetFrame(); - /// Output_GetCameraCount OutputGCC = MyClient.GetCameraCount(); - /// if( OutputGCC.Result == Success && OutputGCC.CameraCount > 0 ) - /// { - /// Output_GetCameraName OutputGCN; - /// OutputGCN = MyClient.GetCameraName( 0 ); - /// if( OutputGCN.Result == Success ) - /// { - /// Output_GetIsVideoCamera Output_GCV = MyClient.GetIsVideoCamera( OutputGCN.CameraName ); - /// } - /// } - /// ----- - /// - /// \return An Output_ class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + InvalidCameraName - Output_GetIsVideoCamera GetIsVideoCamera( const std::string & CameraName ) const; - - /// Return the number of centroids reported by a named camera. The centroid data needs to be enabled to get the number of centroids. - /// - /// See Also: GetCameraCount(), GetCameraName(), GetCentroidPosition() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableCentroidData ( pClient ); - /// Client_GetFrame( pClient ); - /// COutput_GetCameraCount CameraCount; - /// Client_GetCameraCount(pClient, &CameraCount); - /// if( CameraCount.Result == Success && CameraCount.CameraCount > 0 ) - /// { - /// char CameraName[128]; - /// CEnum Output = Client_GetCameraName(pClient, 0, 128, CameraName); - /// if ( Output == Success ) - /// { - /// COutput_GetCentroidCount CentroidCount; - /// Client_GetCentroidCount(pClient, CameraName, &CentroidCount ); - /// } - /// } - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableCentroidData (); - /// MyClient.GetFrame(); - /// Output_GetCameraCount OutputGCC = MyClient.GetCameraCount(); - /// for( unsigned int CameraIndex = 0; CameraIndex < OutputGCC.CameraCount; ++CameraIndex ) - /// { - /// Output_GetCameraName OutputGCN = MyClient.GetCameraName( CameraIndex ); - /// Output_GetCentroidCount OutputGCeC = MyClient.GetCentroidCount( OutputGCN.CameraName ); - /// // OutputGCeC.Result == Success - /// // OutputGCeC.CentroidCount >= 0 - /// } - /// - /// MATLAB example - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableCentroidData(); - /// MyClient.GetFrame(); - /// OutputGCC = MyClient.GetCameraCount(); - /// for CameraIndex = 1:OutputGCC.CameraCount - /// OutputGCN = MyClient.GetCameraName( CameraIndex ); - /// OutputGCeC = MyClient.GetCentroidCount( OutputGCN.CameraName ) - /// % OutputGCeC.Result == Success - /// % OutputGCeC.CentroidCount >= 0 - /// End - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableCentroidData (); - /// MyClient.GetFrame(); - /// Output_GetCameraCount OutputGCC = MyClient.GetCameraCount(); - /// for( unsigned int CameraIndex = 0; CameraIndex < OutputGCC.CameraCount; ++CameraIndex ) - /// { - /// OutputGCN = MyClient.GetCameraName( CameraIndex ); - /// OutputGCeC = MyClient.GetCentroidCount( OutputGCN.CameraName ) - /// // OutputGCeC.Result == Success - /// // OutputGCeC.CentroidCount >= 0 - /// } - /// ----- - /// - /// - /// - /// \param CameraName The name of the camera. - /// \return An Output_GetCentroidCount class containing the result of the operation and the number of centroids. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidCameraName - Output_GetCentroidCount GetCentroidCount( const std::string & CameraName ) const; - - /// Return the position and radius of the centroid in camera coordinates. The centroid data needs to be enabled to get the centroid position and radius. - /// - /// See Also: GetCameraCount(), GetCameraName(), GetCentroidCount() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableCentroidData ( pClient ); - /// Client_GetFrame( pClient ); - /// char CameraName[128]; - /// CEnum Output = Client_GetCameraName(pClient, 0, 128, CameraName); - /// COutput_GetCentroidPosition CentroidPosition; - /// Client_GetCentroidPosition(pClient, CameraName, 0, &CentroidPosition ); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// A valid CameraName is obtained from GetCameraName( CameraIndex ) - /// A valid CentroidIndex is between 0 and GetCentroidCount( CameraName )-1 - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableCentroidData (); - /// MyClient.GetFrame(); - /// Output_GetCameraName OutputGCN = MyClient.GetCameraName( 0 ); - /// Output_GetCentroidPosition Output = MyClient.GetCentroidPosition( OutputGCN.CameraName, 0 ); - /// - /// MATLAB example - /// - /// A valid CameraName is obtained from GetCameraName( CameraIndex ) - /// A valid CentroidIndex is between 1 and GetCentroidCount( CameraName ) - /// % [Output] = GetCentroidPosition( CameraName, CentroidIndex ) - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableCentroidData (); - /// MyClient.GetFrame(); - /// OutputGCN = MyClient.GetCameraName( 1 ); - /// Output = MyClient.GetCentroidPosition( OutputGCN.CameraName, 1 ); - /// - /// .NET example - /// - /// A valid CameraName is obtained from GetCameraName( CameraIndex ) - /// A valid CentroidIndex is between 0 and GetCentroidCount( CameraName )-1 - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableCentroidData (); - /// MyClient.GetFrame(); - /// Output_GetCameraName OutputGCN = MyClient.GetCameraName( 0 ); - /// Output_GetCentroidPosition Output = MyClient.GetCentroidPosition( OutputGCN.CameraName, 0 ); - /// ----- - /// - /// - /// - /// \param CameraName The name of the camera. - /// \param CentroidIndex The index of the centroid. - /// \return An Output_GetCentroidPosition class containing the result of the operation, the position of the centroid and the radius of the centroid. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidCameraName - /// + InvalidIndex - Output_GetCentroidPosition GetCentroidPosition( const std::string & CameraName, const unsigned int CentroidIndex ) const; - - /// Return the weight of the centroid. The centroid data needs to be enabled to get the centroid weight. Only supported by Tracker - weights will be 1.0 for all centroids if Low Jitter mode is not enabled. - /// - /// See Also: GetCameraCount(), GetCameraName(), GetCentroidCount() - /// - /// - /// C example - /// - /// CClient * pClient = Client_Create(); - /// Client_Connect( pClient, "localhost" ); - /// Client_EnableCentroidData ( pClient ); - /// Client_GetFrame( pClient ); - /// char CameraName[128]; - /// CEnum Output = Client_GetCameraName(pClient, 0, 128, CameraName); - /// COutput_GetCentroidWeight CentroidWeight; - /// Client_GetCentroidWeight(pClient, CameraName, 0, &CentroidWeight ); - /// Client_Destroy( pClient ); - /// - /// C++ example - /// - /// A valid CameraName is obtained from GetCameraName( CameraIndex ) - /// A valid CentroidIndex is between 0 and GetCentroidCount( CameraName )-1 - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableCentroidData (); - /// MyClient.GetFrame(); - /// Output_GetCameraName OutputGCN = MyClient.GetCameraName( 0 ); - /// Output_GetCentroidWeight Output = MyClient.GetCentroidWeight( OutputGCN.CameraName, 0 ); - /// - /// MATLAB example - /// - /// A valid CameraName is obtained from GetCameraName( CameraIndex ) - /// A valid CentroidIndex is between 1 and GetCentroidCount( CameraName ) - /// % [Output] = GetCentroidWeight( CameraName, CentroidIndex ) - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableCentroidData (); - /// MyClient.GetFrame(); - /// OutputGCN = MyClient.GetCameraName( 1 ); - /// Output = MyClient.GetCentroidWeight( OutputGCN.CameraName, 1 ); - /// - /// .NET example - /// - /// A valid CameraName is obtained from GetCameraName( CameraIndex ) - /// A valid CentroidIndex is between 0 and GetCentroidCount( CameraName )-1 - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableCentroidData (); - /// MyClient.GetFrame(); - /// Output_GetCameraName OutputGCN = MyClient.GetCameraName( 0 ); - /// Output_GetCentroidWeight Output = MyClient.GetCentroidWeight( OutputGCN.CameraName, 0 ); - /// ----- - /// - /// - /// - /// \param CameraName The name of the camera. - /// \param CentroidIndex The index of the centroid. - /// \return An Output_GetCentroidWeight class containing the result of the operation and the weight of the centroid. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidCameraName - /// + InvalidIndex - Output_GetCentroidWeight GetCentroidWeight( const std::string & CameraName, const unsigned int CentroidIndex ) const; - - - /// Obtain the number of greyscale blobs that are available for the specified camera. - /// - /// See Also: GetGreyscaleBlob(), EnableGreyscaleData() - /// - /// - /// C example - /// - /// Not implemented - /// - /// - /// C++ example - /// - /// A valid camera name may be obtained from GetCameraName( CameraIndex ) - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableGreyscaleData (); - /// MyClient.GetFrame(); - /// Output_GetCameraName CameraName = MyClient.GetCameraName( 0 ); - /// Output_GetGreyscaleBlobCount Output = MyClient.GetGreyscaleBlobCount( CameraName.CameraName ); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// - /// .NET example - /// - /// A valid camera name may be obtained from GetCameraName( CameraIndex ) - /// A valid blob index is between 0 and GetGreyscaleBlobCount() - 1 - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableGreyscaleData (); - /// MyClient.GetFrame(); - /// Output_GetCameraName CameraName = MyClient.GetCameraName( 0 ); - /// Output_GetGreyscaleBlob GreyscaleData = MyClient.GetGreyscaleBlob( CameraName.CameraName, 0 ); - /// ----- - /// - /// \return An Output_GetGreyscaleBlobCount class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + InvalidCameraName - Output_GetGreyscaleBlobCount GetGreyscaleBlobCount( const std::string & CameraName ) const; - - /// Obtains greyscale blob data for the specified camera and blob index. - /// - /// See Also: GetGreyscaleBlobCount(), EnableGreyscaleData() - /// - /// - /// C example - /// - /// Not implemented - /// - /// - /// C++ example - /// - /// A valid camera name may be obtained from GetCameraName( CameraIndex ) - /// A valid blob index is between 0 and GetGreyscaleBlobCount() - 1 - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableGreyscaleData (); - /// MyClient.GetFrame(); - /// Output_GetCameraName CameraName = MyClient.GetCameraName( 0 ); - /// Output_GetGreyscaleBlob GreyscaleData = MyClient.GetGreyscaleBlob( CameraName.CameraName, 0 ); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// - /// .NET example - /// - /// A valid camera name may be obtained from GetCameraName( CameraIndex ) - /// A valid blob index is between 0 and GetGreyscaleBlobCount() - 1 - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableGreyscaleData (); - /// MyClient.GetFrame(); - /// Output_GetCameraName CameraName = MyClient.GetCameraName( 0 ); - /// Output_GetGreyscaleBlob GreyscaleData = MyClient.GetGreyscaleBlob( CameraName.CameraName, 0 ); - /// ----- - /// - /// \return An Output_GetGreyscaleBlob class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + InvalidCameraName - /// + InvalidIndex - Output_GetGreyscaleBlob GetGreyscaleBlob( const std::string & CameraName, const unsigned int i_BlobIndex ) const; - - /// Obtains video data for the specified camera. - /// - /// See Also: - - /// - /// - /// C example - /// - /// Not implemented - /// - /// - /// C++ example - /// - /// A valid camera name may be obtained from GetCameraName( CameraIndex ) - /// A valid blob index is between 0 and GetGreyscaleBlobCount() - 1 - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableVideoData (); - /// MyClient.GetFrame(); - /// Output_GetCameraName CameraName = MyClient.GetCameraName( 0 ); - /// Output_GetVideoFrame VideoData = MyClient.GetVideoFrame( CameraName.CameraName ); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableVideoData (); - /// MyClient.GetFrame(); - /// Output_GetCameraName CameraName = MyClient.GetCameraName( 0 ); - /// Output_GetVideoFrame VideoData = MyClient.GetVideoFrame( CameraName.CameraName ); /// ----- - /// ----- - /// - /// \return An Output_GetVideoFrame class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + InvalidCameraName - Output_GetVideoFrame GetVideoFrame( const std::string & CameraName ) const; - - /// Add a filter to allow centroid, blob or video data being transmitted only for the specified cameras. - /// - /// See Also: GetGreyscaleBlobCount(), GetGreyscaleBlob(), GetCentroidCount(), GetCentroidPosition(), GetCentroidWeight() - /// - /// - /// C example - /// - /// Not implemented - /// - /// - /// C++ example - /// - /// A valid camera name may be obtained from GetCameraName( CameraIndex ) - /// A valid camera id may be obtained from GetCameraId( CameraName ) - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetCameraName CameraName = MyClient.GetCameraName( 0 ); - /// Output_GetCameraId CameraId = MyClient.GetCameraName( CameraName.CameraName ); - /// std::vector< unsigned int > ReceiveCentroids; - /// ReceiveCentroids.push_back( CameraId.CameraId ); - /// std::vector< unsigned int > ReceiveBlobs; - /// ReceiveBlobs.push_back( CameraId.CameraId ); - /// std::vector< unsigned int > ReceiveVideo; - /// ReceiveVideo.push_back( CameraId.CameraId ); - /// Output_SetCameraFilter FilterResults = - /// MyClient.SetCameraFilter( ReceiveCentroids, ReceiveBlobs, ReceiveVideo ); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetCameraName CameraName = MyClient.GetCameraName( 0 ); - /// Output_GetCameraId CameraId = MyClient.GetCameraName( CameraName.CameraName ); - /// List< unsigned int >^ ReceiveCentroids = gcnew List< unsigned int >(); - /// ReceiveCentroids.Add( CameraId.CameraId ); - /// List< unsigned int >^ ReceiveBlobs = gcnew List< unsigned int >(); - /// ReceiveBlobs.Add( CameraId.CameraId ); - /// List< unsigned int >^ ReceiveVideo = gcnew List< unsigned int >(); - /// ReceiveVideo.Add( CameraId.CameraId ); - /// Output_SetCameraFilter FilterResults = - /// MyClient.SetCameraFilter( ReceiveCentroids, ReceiveBlobs, ReceiveVideo ); /// ----- - /// ----- - /// - /// \return An Output_SetCameraFilter class containing the result of the operation. - /// - The Result will be: - /// + Success - Output_SetCameraFilter SetCameraFilter( const std::vector< unsigned int > & CameraIdsForCentroids, - const std::vector< unsigned int > & CameraIdsForBlobs, - const std::vector< unsigned int > & CameraIdsForVideo ); - - - private: - ClientImpl * m_pClientImpl; - }; -} // End of namespace CPP -} // End of namespace ViconDataStreamSDK diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/DataStreamRetimingClient.cpp b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/DataStreamRetimingClient.cpp deleted file mode 100644 index 033ed24f8..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/DataStreamRetimingClient.cpp +++ /dev/null @@ -1,458 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#define _EXPORTING - -#include "DataStreamRetimingClient.h" - -#include -#include - -#include "StringFactory.h" -#include "CoreAdapters.h" - -namespace ViconDataStreamSDK -{ - namespace CPP - { - - // This class just wraps something whose identity we don't want to expose - // in our header file - class RetimingClientImpl - { - public: - RetimingClientImpl() - : m_pCoreClient(new ViconDataStreamSDK::Core::VClient()) - , m_pCoreRetimingClient(new ViconDataStreamSDK::Core::VRetimingClient(*m_pCoreClient)) - , m_pStringFactory(new VStringFactory()) - { - } - - std::shared_ptr< ViconDataStreamSDK::Core::VClient > m_pCoreClient; - std::shared_ptr< ViconDataStreamSDK::Core::VRetimingClient > m_pCoreRetimingClient; - std::shared_ptr< VStringFactory > m_pStringFactory; - }; - CLASS_DECLSPEC - RetimingClient::RetimingClient() - : m_pClientImpl(new RetimingClientImpl()) - { - } - - CLASS_DECLSPEC - RetimingClient::~RetimingClient() - { - delete m_pClientImpl; - m_pClientImpl = 0; - } - - CLASS_DECLSPEC - Output_GetVersion RetimingClient::GetVersion() const - { - Output_GetVersion Output; - m_pClientImpl->m_pCoreClient->GetVersion(Output.Major, - Output.Minor, - Output.Point); - - return Output; - } - - CLASS_DECLSPEC - Output_Connect RetimingClient::Connect(const String & HostName, double FrameRate) - { - - std::shared_ptr< ViconCGStreamClientSDK::ICGClient > pClient(ViconCGStreamClientSDK::ICGClient::CreateCGClient(), - boost::bind(&ViconCGStreamClientSDK::ICGClient::Destroy, _1)); - Output_Connect Output; - Output.Result = Adapt(m_pClientImpl->m_pCoreRetimingClient->Connect(pClient, HostName)); - - if (Output.Result == Result::Success && FrameRate > 0) - { - Output.Result = Adapt(m_pClientImpl->m_pCoreRetimingClient->StartOutput(FrameRate)); - } - - return Output; - } - - CLASS_DECLSPEC - Output_Disconnect RetimingClient::Disconnect() - { - Output_Disconnect Output; - Output.Result = Adapt(m_pClientImpl->m_pCoreRetimingClient->Disconnect()); - return Output; - } - - CLASS_DECLSPEC - Output_IsConnected RetimingClient::IsConnected() const - { - Output_IsConnected Output; - Output.Connected = m_pClientImpl->m_pCoreClient->IsConnected(); - return Output; - } - - - CLASS_DECLSPEC - Output_SetAxisMapping RetimingClient::SetAxisMapping(const Direction::Enum XAxis, const Direction::Enum YAxis, const Direction::Enum ZAxis) - { - Output_SetAxisMapping Output; - Output.Result = Adapt(m_pClientImpl->m_pCoreClient->SetAxisMapping(Adapt(XAxis), Adapt(YAxis), Adapt(ZAxis))); - return Output; - } - - CLASS_DECLSPEC - Output_GetAxisMapping RetimingClient::GetAxisMapping() const - { - ViconDataStreamSDK::Core::Direction::Enum _XAxis; - ViconDataStreamSDK::Core::Direction::Enum _YAxis; - ViconDataStreamSDK::Core::Direction::Enum _ZAxis; - - m_pClientImpl->m_pCoreClient->GetAxisMapping(_XAxis, _YAxis, _ZAxis); - - Output_GetAxisMapping Output; - Output.XAxis = Adapt(_XAxis); - Output.YAxis = Adapt(_YAxis); - Output.ZAxis = Adapt(_ZAxis); - - return Output; - } - - - CLASS_DECLSPEC - Output_UpdateFrame RetimingClient::UpdateFrame(double i_Offset) - { - Output_UpdateFrame Output; - Output.Result = Adapt(m_pClientImpl->m_pCoreRetimingClient->UpdateFrame(i_Offset)); - return Output; - } - - CLASS_DECLSPEC - Output_WaitForFrame RetimingClient::WaitForFrame() - { - Output_WaitForFrame Output; - Output.Result = Adapt(m_pClientImpl->m_pCoreRetimingClient->WaitForFrame()); - return Output; - } - - CLASS_DECLSPEC - Output_GetSubjectCount RetimingClient::GetSubjectCount() const - { - Output_GetSubjectCount Output; - Output.Result = Adapt(m_pClientImpl->m_pCoreRetimingClient->GetSubjectCount(Output.SubjectCount)); - - return Output; - } - - CLASS_DECLSPEC - Output_GetSubjectName RetimingClient::GetSubjectName(const unsigned int SubjectIndex) const - { - Output_GetSubjectName Output; - - std::string SubjectName; - Output.Result = Adapt(m_pClientImpl->m_pCoreRetimingClient->GetSubjectName(SubjectIndex, SubjectName)); - Output.SubjectName.Set(SubjectName.c_str(), *m_pClientImpl->m_pStringFactory.get()); - - return Output; - } - - CLASS_DECLSPEC - Output_GetSubjectRootSegmentName RetimingClient::GetSubjectRootSegmentName(const String & SubjectName) const - { - Output_GetSubjectRootSegmentName Output; - std::string SegmentName; - - Output.Result = Adapt(m_pClientImpl->m_pCoreRetimingClient->GetSubjectRootSegmentName(SubjectName, SegmentName)); - - if (Output.Result != Result::Success) - { - std::cout << "Get Root Segment Name failed " << Output.Result << std::endl; - } - - Output.SegmentName.Set(SegmentName.c_str(), *m_pClientImpl->m_pStringFactory.get()); - return Output; - } - - CLASS_DECLSPEC - Output_GetSegmentCount RetimingClient::GetSegmentCount(const String & SubjectName) const - { - Output_GetSegmentCount Output; - Output.Result = Adapt(m_pClientImpl->m_pCoreRetimingClient->GetSegmentCount(SubjectName, Output.SegmentCount)); - - return Output; - } - - CLASS_DECLSPEC - Output_GetSegmentName RetimingClient::GetSegmentName(const String & SubjectName, - const unsigned int SegmentIndex) const - { - Output_GetSegmentName Output; - std::string SegmentName; - - Output.Result = Adapt(m_pClientImpl->m_pCoreRetimingClient->GetSegmentName(SubjectName, SegmentIndex, SegmentName)); - - Output.SegmentName.Set(SegmentName.c_str(), *m_pClientImpl->m_pStringFactory.get()); - return Output; - } - - CLASS_DECLSPEC - Output_GetSegmentChildCount RetimingClient::GetSegmentChildCount(const String & SubjectName, - const String & SegmentName) const - { - Output_GetSegmentChildCount Output; - Output.Result = Adapt(m_pClientImpl->m_pCoreRetimingClient->GetSegmentChildCount(SubjectName, SegmentName, Output.SegmentCount)); - - return Output; - } - - CLASS_DECLSPEC - Output_GetSegmentChildName RetimingClient::GetSegmentChildName(const String & SubjectName, - const String & SegmentName, - const unsigned int SegmentIndex) const - { - Output_GetSegmentChildName Output; - std::string SegmentChildName; - - Output.Result = Adapt(m_pClientImpl->m_pCoreRetimingClient->GetSegmentChildName(SubjectName, SegmentName, SegmentIndex, SegmentChildName)); - - Output.SegmentName.Set(SegmentChildName.c_str(), *m_pClientImpl->m_pStringFactory.get()); - return Output; - } - - CLASS_DECLSPEC - Output_GetSegmentParentName RetimingClient::GetSegmentParentName(const String & SubjectName, - const String & SegmentName) const - { - Output_GetSegmentParentName Output; - std::string SegmentParentName; - - Output.Result = Adapt(m_pClientImpl->m_pCoreRetimingClient->GetSegmentParentName(SubjectName, SegmentName, SegmentParentName)); - - Output.SegmentName.Set(SegmentParentName.c_str(), *m_pClientImpl->m_pStringFactory.get()); - return Output; - } - - CLASS_DECLSPEC - Output_GetSegmentStaticTranslation RetimingClient::GetSegmentStaticTranslation(const String & SubjectName, - const String & SegmentName) const - { - Output_GetSegmentStaticTranslation Output; - Output.Result = Adapt(m_pClientImpl->m_pCoreRetimingClient->GetSegmentStaticTranslation(SubjectName, - SegmentName, - Output.Translation)); - - return Output; - } - - CLASS_DECLSPEC - Output_GetSegmentStaticRotationHelical RetimingClient::GetSegmentStaticRotationHelical(const String & SubjectName, - const String & SegmentName) const - { - Output_GetSegmentStaticRotationHelical Output; - Output.Result = Adapt(m_pClientImpl->m_pCoreRetimingClient->GetSegmentStaticRotationHelical(SubjectName, - SegmentName, - Output.Rotation)); - - return Output; - } - - CLASS_DECLSPEC - Output_GetSegmentStaticRotationMatrix RetimingClient::GetSegmentStaticRotationMatrix(const String & SubjectName, - const String & SegmentName) const - { - Output_GetSegmentStaticRotationMatrix Output; - Output.Result = Adapt(m_pClientImpl->m_pCoreRetimingClient->GetSegmentStaticRotationMatrix(SubjectName, - SegmentName, - Output.Rotation)); - - return Output; - } - - CLASS_DECLSPEC - Output_GetSegmentStaticRotationQuaternion RetimingClient::GetSegmentStaticRotationQuaternion(const String & SubjectName, - const String & SegmentName) const - { - Output_GetSegmentStaticRotationQuaternion Output; - Output.Result = Adapt(m_pClientImpl->m_pCoreRetimingClient->GetSegmentStaticRotationQuaternion(SubjectName, - SegmentName, - Output.Rotation)); - - return Output; - } - - CLASS_DECLSPEC - Output_GetSegmentStaticRotationEulerXYZ RetimingClient::GetSegmentStaticRotationEulerXYZ( const String & SubjectName, - const String & SegmentName) const - { - Output_GetSegmentStaticRotationEulerXYZ Output; - Output.Result = Adapt(m_pClientImpl->m_pCoreRetimingClient->GetSegmentStaticRotationEulerXYZ(SubjectName, - SegmentName, - Output.Rotation)); - - return Output; - } - - CLASS_DECLSPEC - Output_GetSegmentGlobalTranslation RetimingClient::GetSegmentGlobalTranslation( const String & SubjectName, - const String & SegmentName) const - { - Output_GetSegmentGlobalTranslation Output; - Output.Result = Adapt(m_pClientImpl->m_pCoreRetimingClient->GetSegmentGlobalTranslation(SubjectName, - SegmentName, - Output.Translation, - Output.Occluded)); - return Output; - } - - CLASS_DECLSPEC - Output_GetSegmentGlobalRotationHelical RetimingClient::GetSegmentGlobalRotationHelical( const String & SubjectName, - const String & SegmentName) const - { - Output_GetSegmentGlobalRotationHelical Output; - Output.Result = Adapt(m_pClientImpl->m_pCoreRetimingClient->GetSegmentGlobalRotationHelical(SubjectName, - SegmentName, - Output.Rotation, - Output.Occluded)); - return Output; - } - - CLASS_DECLSPEC - Output_GetSegmentGlobalRotationMatrix RetimingClient::GetSegmentGlobalRotationMatrix(const String & SubjectName, - const String & SegmentName) const - { - Output_GetSegmentGlobalRotationMatrix Output; - Output.Result = Adapt(m_pClientImpl->m_pCoreRetimingClient->GetSegmentGlobalRotationMatrix(SubjectName, - SegmentName, - Output.Rotation, - Output.Occluded)); - return Output; - } - - CLASS_DECLSPEC - Output_GetSegmentGlobalRotationQuaternion RetimingClient::GetSegmentGlobalRotationQuaternion( const String & SubjectName, - const String & SegmentName) const - { - Output_GetSegmentGlobalRotationQuaternion Output; - Output.Result = Adapt(m_pClientImpl->m_pCoreRetimingClient->GetSegmentGlobalRotationQuaternion(SubjectName, - SegmentName, - Output.Rotation, - Output.Occluded)); - return Output; - } - - CLASS_DECLSPEC - Output_GetSegmentGlobalRotationEulerXYZ RetimingClient::GetSegmentGlobalRotationEulerXYZ( const String & SubjectName, - const String & SegmentName) const - { - Output_GetSegmentGlobalRotationEulerXYZ Output; - Output.Result = Adapt(m_pClientImpl->m_pCoreRetimingClient->GetSegmentGlobalRotationEulerXYZ(SubjectName, - SegmentName, - Output.Rotation, - Output.Occluded)); - return Output; - } - - CLASS_DECLSPEC - Output_GetSegmentLocalTranslation RetimingClient::GetSegmentLocalTranslation( const String & SubjectName, - const String & SegmentName) const - { - Output_GetSegmentLocalTranslation Output; - Output.Result = Adapt(m_pClientImpl->m_pCoreRetimingClient->GetSegmentLocalTranslation(SubjectName, - SegmentName, - Output.Translation, - Output.Occluded)); - return Output; - } - - CLASS_DECLSPEC - Output_GetSegmentLocalRotationHelical RetimingClient::GetSegmentLocalRotationHelical( const String & SubjectName, - const String & SegmentName) const - { - Output_GetSegmentLocalRotationHelical Output; - Output.Result = Adapt(m_pClientImpl->m_pCoreRetimingClient->GetSegmentLocalRotationHelical(SubjectName, - SegmentName, - Output.Rotation, - Output.Occluded)); - return Output; - } - - CLASS_DECLSPEC - Output_GetSegmentLocalRotationMatrix RetimingClient::GetSegmentLocalRotationMatrix( const String & SubjectName, - const String & SegmentName) const - { - Output_GetSegmentLocalRotationMatrix Output; - Output.Result = Adapt(m_pClientImpl->m_pCoreRetimingClient->GetSegmentLocalRotationMatrix(SubjectName, - SegmentName, - Output.Rotation, - Output.Occluded)); - return Output; - } - - CLASS_DECLSPEC - Output_GetSegmentLocalRotationQuaternion RetimingClient::GetSegmentLocalRotationQuaternion( const String & SubjectName, - const String & SegmentName) const - { - Output_GetSegmentLocalRotationQuaternion Output; - Output.Result = Adapt(m_pClientImpl->m_pCoreRetimingClient->GetSegmentLocalRotationQuaternion(SubjectName, - SegmentName, - Output.Rotation, - Output.Occluded)); - return Output; - } - - CLASS_DECLSPEC - Output_GetSegmentLocalRotationEulerXYZ RetimingClient::GetSegmentLocalRotationEulerXYZ( const String & SubjectName, - const String & SegmentName) const - { - Output_GetSegmentLocalRotationEulerXYZ Output; - Output.Result = Adapt(m_pClientImpl->m_pCoreRetimingClient->GetSegmentLocalRotationEulerXYZ(SubjectName, - SegmentName, - Output.Rotation, - Output.Occluded)); - return Output; - } - - - CLASS_DECLSPEC - void RetimingClient::SetOutputLatency( double i_OutputLatency ) - { - m_pClientImpl->m_pCoreRetimingClient->SetOutputLatency(i_OutputLatency); - } - - CLASS_DECLSPEC - double RetimingClient::OutputLatency() const - { - return m_pClientImpl->m_pCoreRetimingClient->OutputLatency(); - } - - CLASS_DECLSPEC - void RetimingClient::SetMaximumPrediction( double i_MaxPrediction ) - { - m_pClientImpl->m_pCoreRetimingClient->SetMaximumPrediction(i_MaxPrediction); - } - - CLASS_DECLSPEC - double RetimingClient::MaximumPrediction() const - { - return m_pClientImpl->m_pCoreRetimingClient->MaximumPrediction(); - } - } - } diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/DataStreamRetimingClient.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/DataStreamRetimingClient.h deleted file mode 100644 index addd763c4..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/DataStreamRetimingClient.h +++ /dev/null @@ -1,1884 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -// \file -// The SDK retiming client and supporting definitions. - -#include -#include -#include -#include - -#include "IDataStreamClientBase.h" - -#include -#include - -namespace ViconDataStreamSDK -{ - namespace CPP - { - class RetimingClientImpl; - - /// \class RetimingClient - /// \brief The re-timing client class for C++. - /// Vicon DataStream SDK Re-Timing Client - /// =========================== - /// Intended uses - /// ------------- - /// The Vicon DataStream re-timing client provides calls to obtain subject data from the - /// DataStream with minimal latency and temporal jitter. - /// When UpdateFrame() is called, the client uses re-timed data that has been linearly interpolated - /// from an internal buffer to predict the position of each segment to the current time. - /// - /// The system and network latencies are used when determining the amount of prediction required. - /// If additional prediction is required, for example for use in a VR system where an additional latency - /// is present due to rendering and display latency; this may be requested in the call to UpdateFrame(). - /// - /// The user will either call UpdateFrame(), which will update the current frame state to - /// the time of calling and return immediately. This is intended for use in systems where you require - /// subject data positions at times driven by an external clock. - /// - /// If you do not have an external clock, and require behavior similar to that of the standard DataStream - /// client running in ServerPush streaming mode, than the system may be configured to provide frame data - /// at a consistent frame rate by providing a frame rate to the Connect() call. The user will then call - /// WaitForFrame(), which will block in a similar method to Client::GetFrame(), but using retimed data - /// in order to keep the frame period very consistent. - /// - /// Examples of use - /// -------------- - /// - /// If you are using the client in a situation where you need to obtain the position of subjects in - /// - /// ViconDataStreamSDK::CPP::RetimingClient _MyClient; - /// _MyClient.Connect( "localhost" ); - /// - /// // example render method. Real code would probably cache the subject and segment names and bind - /// // them to a model, so this code would iterate over the model and update the joint positions. - /// void render() - /// { - /// _MyClient.UpdateFrame(); - /// - /// Output_GetSubjectCount SubjectCount = _MyClient.GetSubjectCount(); - /// if (SubjectCount.Result == Result::Success) - /// { - /// for (unsigned int SubjectIndex = 0; SubjectIndex < SubjectCount.SubjectCount; ++SubjectIndex) - /// { - /// Output_GetSubjectName SubjectName = _MyClient.GetSubjectName(SubjectIndex); - /// if (SubjectName.Result == Result::Success) - /// { - /// Output_GetSegmentCount SegmentCount = _MyClient.GetSegmentCount(SubjectName.SubjectName); - /// if (SegmentCount.Result == Result::Success) - /// { - /// for (unsigned int SegmentIndex = 0; SegmentIndex < SegmentCount.SegmentCount; ++SegmentIndex) - /// { - /// Output_GetSegmentName SegmentName = - /// _MyClient.GetSegmentName(SubjectName.SubjectName, SegmentIndex); - /// if (SegmentName.Result == Result::Success) - /// { - /// Output_GetSegmentGlobalRotationQuaternion SegmentRotation = - /// _MyClient.GetSegmentGlobalRotationQuaternion - /// (SubjectName.SubjectName, SegmentName.SegmentName); - /// if (SegmentRotation.Result == Result::Success && !SegmentRotation.Occluded) - /// { - /// // use the segment rotation - /// } - /// } - /// } - /// } - /// } - /// } - /// } - /// - /// - /// If using the client where there is no render call and you require your own timing. - /// - /// ViconDataStreamSDK::CPP::RetimingClient _MyClient; - /// // Request a retimed update frame rate of 90Hz. - /// _MyClient.Connect( "localhost", 90 ); - /// while( _MyClient.IsConnected() ) - /// { - /// Output_WaitForFrame WaitOutput = _MyClient.WaitForFrame(); - /// if( WaitOutput.Result == Result::Success ) - /// { - /// // iterate over subjects and segments and obtain the joint positions and rotations as above. - /// } - /// } - /// - /// For a more detailed example, see the ViconDataStreamSDK_CPPRetimingTest example. The SimpleViewer application - /// also provides an example of re-timing client use in a practical context. - class CLASS_DECLSPEC RetimingClient : public IDataStreamClientBase - { - public: - - /// Construction. - /// Instances of the Vicon Data Stream RetimingClient create a DataStreamClient internally that manages the connection - /// to the data stream. - /// - /// The RetimingClient will set the underlying client up to receive the required data from the stream and to set the - /// correct data delivery mode, so it is not necessary to set this up manually. - /// - /// - /// C example - /// - /// // The C version uses explicit creation methods - /// - /// CClient * pClient = RetimingClient_Create(); - /// // C Client functions take the client as a parameter - /// CBool ok = RetimingClient_SomeFunction( pClient, Args ); - /// // The C client needs to be explicitly destroyed - /// RetimingClient_Destroy( pClient ); - /// - /// C++ example - /// - /// // The C++ version of the SDK is object oriented, so use the class constructor. - /// - /// { - /// ViconDataStreamSDK::CPP::RetimingClient StackRetimingClient; - /// Output_SomeFunction Output = StackRetimingClient.SomeFunction(); - /// // ... - /// } - /// // Client is implicitly destroyed as it goes out of scope. - /// - /// // Alternatively the Client can be made on the heap. - /// - /// ViconDataStreamSDK::CPP::RetimingClient * pHeapRetimingClient - /// = new ViconDataStreamSDK::CPP::RetimingClient(); - /// Output_SomeFunction Output = pHeapRetimingClient->SomeFunction( Input ); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// // .NET is object oriented, so use the class constructor. - /// - /// // Because objects are lazily garbage collected, your instance may outlive the - /// // last reference to it for some time. - /// - /// // If the instance is pre-fetching frame data for you, then it can still use CPU - /// // and network bandwidth. - /// - /// // Consider explicitly disconnecting prior to destruction. - /// - /// ViconDataStreamSDK.DotNET.RetimingClient pHeapClient - /// = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// Output_SomeFunction Output = pHeapClient.SomeFunction(InputParam); - /// // Signal to the garbage collector that it can clean up - /// pHeapClient.Disconnect(); - /// pHeapClient = null; - /// ----- - RetimingClient(); - - - /// Destruction. - /// Destruction will Disconnect if required. - /// - /// See RetimingClient::RetimingClient for an example. - virtual ~RetimingClient(); - - /// Get the version of the Vicon DataStream SDK. - /// + **Major** - /// When this number increases, we break backward compatibility with previous major versions. - /// + **Minor** - /// When this number increases, we have probably added new functionality to the SDK without breaking - /// backward compatibility with previous versions. - /// + **Point** - /// When this number increases, we have introduced a bug fix or performance enhancement without breaking - /// backward compatibility with previous versions. - /// - /// The function can be called without the client being connected. - /// - /// - /// C example - /// - /// CRetimingClient * pClient = RetimingClient_Create(); - /// COutput_GetVersion Output = RetimingClient_GetVersion( pClient ); - /// RetimingClient_Destroy( pClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// Output_GetVersion Output = MyClient.GetVersion(); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// Output_GetVersion Output = MyClient.GetVersion(); - /// ----- - /// - /// \return Output_GetVersion class containing the version information. - Output_GetVersion GetVersion() const; - - /// Establish a dedicated connection to a Vicon DataStream Server. - /// - /// See Also: Disconnect(), IsConnected(). - /// - /// The function defaults to connecting on port 801. - /// You can specify an alternate port number after a colon. - /// This is for future compatibility: current products serve data on port 801 only. - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// COutput_Connect Output = RetimingClient_Connect( pRetimingClient, "localhost"); - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// Output_Connect Output = MyClient.Connect( "localhost" ); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// Output_Connect Output = MyClient.Connect( "localhost" ); - /// // Connect with alternative FrameRate parameter - /// Output_Connect Output = MyClient.Connect( "localhost", 90.0 ); - /// ----- - /// - /// \param HostName The DNS-identifiable name, or IP address of the PC hosting the DataStream server. - /// For example: - /// + "localhost" - /// + "MyViconPC:801" - /// + "10.0.0.2" - /// - /// \param FrameRate An optional parameter - if specified, the re-timing client's internal frame output - /// clock will be active. This is implemented by a separate overloaded method on .NET - /// - /// \return An Output_Connect class containing the result of the connect operation. - /// - The Result will be: - /// + Success - /// + InvalidHostName - /// + ClientAlreadyConnected - /// + ClientConnectionFailed - Output_Connect Connect(const String & HostName, double FrameRate = 0.0 ); - - /// Disconnect from the Vicon DataStream Server. - /// - /// See Also: Connect(), IsConnected() - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// RetimingClient_Connect( pRetimingClient, "localhost" ); - /// COutput_Disconnect Output = RetimingClient_Disconnect( pRetimingClient ); - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_Disconnect Output = MyClient.Disconnect(); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// MyClient.Connect( "localhost" ); - /// Output_Disconnect Output = MyClient.Disconnect(); - /// ----- - /// - /// \return An Output_Disconnect class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - Output_Disconnect Disconnect(); - - /// Discover whether client is connected to the Vicon DataStream Server. - /// - /// See Also: Connect(), Disconnect() - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// CBool Output = RetimingClient_IsConnected( pRetimingClient ); - /// // Output == 0 - /// RetimingClient_Connect( pRetimingClient, "localhost" ); - /// Output = RetimingClient_IsConnected( pRetimingClient ); - /// // Output == 1 - /// COutput_Disconnect Output = RetimingClient_Disconnect( pRetimingClient ); - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::CPP::Client MyClient; - /// Output_IsConnected Output = MyClient.IsConnected() - /// // Output.Connected == false - /// MyClient.Connect( "localhost" ); - /// Output_IsConnected Output = MyClient.IsConnected() - /// // Output.Connected == true - /// // (assuming localhost is serving) - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// Output_IsConnected Output = MyClient.IsConnected() - /// // Output.Connected == false - /// MyClient.Connect( "localhost" ); - /// Output_IsConnected Output = MyClient.IsConnected() - /// // Output.Connected == true - /// // (assuming localhost is serving) - /// ----- - /// - /// \return An Output_IsConnected class containing a true value for Connected if you are - /// connected to the stream, otherwise false. - Output_IsConnected IsConnected() const; - - /// Remaps the 3D axis. - /// Vicon Data uses a right-handed coordinate system, with +X forward, +Y left, and +Z up. - /// Other systems use different coordinate systems. The SDK can transform its data into any valid right-handed coordinate system by re-mapping each axis. - /// Valid directions are "Up", "Down", "Left", "Right", "Forward", and "Backward". Note that "Forward" means moving away from you, and "Backward" is moving towards you. - /// Common usages are - /// Z-up: SetAxisMapping( Forward, Left, Up ) - /// Y-up: SetAxisMapping( Forward, Up, Right ) - /// - /// See Also: GetAxisMapping() - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// RetimingClient_Connect( pRetimingClient, "localhost" ); - /// RetimingClient_SetAxisMapping(pRetimingClient, Forward, Left, Up); // Z-up - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// MyClient.SetAxisMapping( ViconDataStreamSDK::CPP::Direction::Forward, - /// ViconDataStreamSDK::CPP::Direction::Left, - /// ViconDataStreamSDK::CPP::Direction::Up ); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.SetAxisMapping( ViconDataStreamSDK.DotNET.Direction.Forward, - /// ViconDataStreamSDK.DotNET.Direction.Left, - /// ViconDataStreamSDK.DotNET.Direction.Up ); - /// ----- - /// \param XAxis Specify the direction of your X axis relative to yourself as the observer. - /// \param YAxis Specify the direction of your Y axis relative to yourself as the observer. - /// \param ZAxis Specify the direction of your Z axis relative to yourself as the observer. - /// - /// \return An Output_SetAxisMapping class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + CoLinearAxes - /// + LeftHandedAxes - Output_SetAxisMapping SetAxisMapping(const Direction::Enum XAxis, const Direction::Enum YAxis, const Direction::Enum ZAxis); - - /// Get the current Axis mapping. - /// - /// See Also: SetAxisMapping() - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// RetimingClient_Connect( pRetimingClient, "localhost" ); - /// RetimingClient_SetAxisMapping(pRetimingClient, Forward, Left, Up); // Z-up - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// Output_GetAxisMapping Output = MyClient.GetAxisMapping(); - /// // Output.XAxis == ViconDataStreamSDK::CPP::Direction::Forward - /// // Output.YAxis == ViconDataStreamSDK::CPP::Direction::Left - /// // Output.ZAxis == ViconDataStreamSDK::CPP::Direction::Up - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// Output_GetAxisMapping Output = MyClient.GetAxisMapping(); - /// // Output.XAxis == ViconDataStreamSDK.DotNET.Direction.Forward - /// // Output.YAxis == ViconDataStreamSDK.DotNET.Direction.Left - /// // Output.ZAxis == ViconDataStreamSDK.DotNET.Direction.Up - /// ----- - /// \return An Output_GetAxisMapping class containing the result of the operation. - /// - The Result will be: - /// + XAxis, YAxis, ZAxis - Output_GetAxisMapping GetAxisMapping() const; - - /// Update the current frame state to represent the position of all active subjects at the - /// current time. - /// - /// The position of each segment is estimated by predicting forwards from the most recent - /// frames received from the DataStream, taking into account the latency reported by the - /// system to determine the amount of prediction required. - /// - /// The results of calls which return details about the current frame state such as GetSubjectCount() - /// and GetSegmentGlobalRotationQuaternion() will all return the stream contents and position at the - /// time that this call was made. - /// - /// If no call to UpdateFrame() is made, calls querying the stream state will return NoFrame - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// CEnum Output = RetimingClient_GetFrame(); // Output == NotConnected - /// RetimingClient_Connect( pRetimingClient, "localhost" ); - /// Output = RetimingClient_UpdateFrame(); // Output == Success - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// Output_UpdateFrame Output; - /// Output = MyClient.UpdateFrame(); // Output.Result == NotConnected - /// MyClient.Connect( "localhost" ); - /// Output = MyClient.UpdateFrame(); // Output.Result == Success - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// Output_UpdateFrame Output; - /// Output = MyClient.UpdateFrame(); // Output.Result == NotConnected - /// MyClient.Connect( "localhost" ); - /// Output = MyClient.UpdateFrame(); // Output.Result == Success - /// Output = MyClient.UpdateFrame(20); // Output.Result == Success - /// ----- - /// - /// \param Offset An additional offset that will be applied to the time at which which the predicted - /// position is calculated. This may be used to compensate for additional delays that - /// are in the users system, such as render delay. This is implemnted in a separate overloaded - /// method in .NET. - /// - /// \return An Output_UpdateFrame class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - Output_UpdateFrame UpdateFrame( double Offset = 0.0 ); - - /// Used when running the retiming client with a specified frame rate. This call will - /// block until the next frame is available, as driven by an internal clock running at - /// the frame rate specified by Connect( Host, FrameRate). The frame data is re-timed to the - /// correct time point - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// CEnum Output = RetimingClient_GetFrame(); // Output == NotConnected - /// RetimingClient_ConnectAndStart( pRetimingClient, "localhost", 200 ); - /// Output = RetimingClient_WaitForFrame(); // Output == Success - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// MyClient.Connect( "localhost", 200 ); - /// Output = MyClient.WaitForFrame(); // Output.Result == Success - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// MyClient.Connect( "localhost", 200 ); - /// Output = MyClient.WaitForFrame(); // Output.Result == Success - /// ----- - /// - /// \return An Output_WaitForFrame class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - Output_WaitForFrame WaitForFrame(); - - /// Return the number of subjects in the DataStream. This information can be used in conjunction with GetSubjectName. - /// - /// See Also: GetSubjectName() - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// RetimingClient_Connect( pRetimingClient, "localhost" ); - /// COutput_GetSubjectCount SubjectCount; - /// RetimingClient_GetSubjectCount(pRetimingClient, &SubjectCount); // SubjectCount.Result == NoFrame - /// // SubjectCount.SubjectCount == 0; - /// RetimingClient_GetFrame( pRetimingClient ); - /// RetimingClient_GetSubjectCount(pRetimingClient, &SubjectCount); // SubjectCount.Result == Success; - /// // SubjectCount.SubjectCount == 0; - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_GetSubjectCount Output; - /// Output = MyClient.GetSubjectCount(); // Output.Result == NoFrame - /// // Ooutput.SubjectCount == 0 - /// MyClient.GetFrame(); - /// Output = MyClient.GetSubjectCount(); // Output.Result == Success - /// // Output.SubjectCount >= 0 - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// MyClient.Connect( "localhost" ); - /// Output_GetSubjectCount Output; - /// Output = MyClient.GetSubjectCount(); // Output.Result == NoFrame - /// // Ooutput.SubjectCount == 0 - /// MyClient.UpdateFrame(); - /// Output = MyClient.GetSubjectCount(); // Output.Result == Success - /// // Output.SubjectCount >= 0 - /// ----- - /// - /// - /// \return An Output_GetSubjectCount class containing the result of the operation and the number of subjects. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - Output_GetSubjectCount GetSubjectCount() const; - - /// Return the name of a subject. This can be passed into segment and marker functions. - /// - /// See Also: GetSubjectCount() - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// RetimingClient_Connect( pRetimingClient, "localhost" ); - /// RetimingClient_GetFrame( pRetimingClient ); - /// char SubjectName[128]; - /// CEnum Output = RetimingClient_GetSubjectName(pRetimingClient, 0, 128, SubjectName); - /// // Output == Success - /// // SubjectName =="AI" - /// Output = RetimingClient_GetSubjectName(pRetimingClient, 1, 128, SubjectName); - /// // Output == Success - /// // SubjectName =="Bob" - /// Output = RetimingClient_GetSubjectName(pRetimingClient, 2, 128, SubjectName); - /// // Output == InvalidIndex - /// // SubjectName =="" - /// - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSubjectCount OutputGSC; - /// OutputGSC = MyClient.GetSubjectCount(); // OutputGSC.Result == Success - /// // OutputGSC.SubjectCount == 2 - /// Output_GetSubjectName OutputGSN; - /// OutputGSN = MyClient.GetSubjectName(0);// OutputGSN.Result == Success - /// // OutputGSN.SubjectName == "Al" - /// OutputGSN = MyClient.GetSubjectName(1);// OutputGSN.Result == Success - /// // OutputGSN .SubjectName == "Bob" - /// OutputGSN = MyClient.GetSubjectName(2);// OutputGSN.Result == InvalidIndex - /// // OutputGSN.SubjectName == "" - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// MyClient.Connect( "localhost" ); - /// MyClient.UpdateFrame(); - /// Output_GetSubjectCount OutputGSC; - /// OutputGSC = MyClient.GetSubjectCount(); // OutputGSC.Result == Success - /// // OutputGSC.SubjectCount == 2 - /// Output_GetSubjectName OutputGSN; - /// OutputGSN = MyClient.GetSubjectName(0);// OutputGSN.Result == Success - /// // OutputGSN.SubjectName == "Al" - /// OutputGSN = MyClient.GetSubjectName(1);// OutputGSN.Result == Success - /// // OutputGSN .SubjectName == "Bob" - /// OutputGSN = MyClient.GetSubjectName(2);// OutputGSN.Result == InvalidIndex - /// // OutputGSN.SubjectName == "" - /// ----- - /// - /// - /// \param SubjectIndex The index of the subject. A valid Subject Index is between 0 and GetSubjectCount()-1. - /// Matlab: A valid Subject Index is between 1 and GetSubjectCount(). - /// \return An Output_GetSubjectName GetSubjectName class containing the result of the operation and the name of the subject. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - Output_GetSubjectName GetSubjectName(const unsigned int SubjectIndex) const; - - /// Return the name of the root segment for a specified subject. This can be passed into segment functions.The root segment is the ancestor of all other segments in the subject. - /// - /// See Also: GetSegmentCount(), GetSegmentParentName(), GetSegmentChildCount(), GetSegmentChildName() - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// RetimingClient_Connect( pRetimingClient, "localhost" ); - /// RetimingClient_EnableSegmentData( pRetimingClient ); - /// RetimingClient_GetFrame( pRetimingClient ); - /// char RootSegment[128]; - /// CEnum Result = RetimingClient_GetSubjectRootSegmentName(pRetimingClient, "Bob", 128, RootSegment); - /// // Result == Success - /// // RootSegment == "Pelvis" - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSubjectRootSegmentName Output; - /// Output = MyClient.GetSubjectRootSegmentName( "Bob" ); - /// // Output.Result == Success - /// // Output.SegmentName == "Pelvis" - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// MyClient.Connect( "localhost" ); - /// MyClient.UpdateFrame(); - /// Output_GetSubjectRootSegmentName Output; - /// Output = MyClient.GetSubjectRootSegmentName( "Bob" ); - /// // Output.Result == Success - /// // Output.SegmentName == "Pelvis" - /// ----- - /// - /// - /// \param SubjectName The name of the subject - /// \return An Output_GetSubjectRootSegmentName class containing the result of the operation and the name of the root segment. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - Output_GetSubjectRootSegmentName GetSubjectRootSegmentName(const String & SubjectName) const; - - /// Return the number of segments for a specified subject in the DataStream. This information can be used in conjunction with GetSegmentName. - /// - /// See Also: GetSubjectName(), GetSegmentName() - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// RetimingClient_Connect( pRetimingClient, "localhost" ); - /// COutput_GetSegmentCount SegmentCount; - /// RetimingClient_GetSegmentCount( pRetimingClient, "Bob", &SegmentCount ); - /// // SegmentCount.Result == NOFrame - /// // SegmentCount.Value == 0 - /// RetimingClient_GetFrame( pRetimingClient ); - /// RetimingClient_GetSegmentCount( pRetimingClient, "AI", &SegmentCount ); - /// // SegmentCount.Result == InvalidSubjectName - /// // SegmentCount.Value == 0 - /// RetimingClient_GetSegmentCount( pRetimingClient, "Bob", &SegmentCount ); - /// // SegmentCount.Result == Success - /// // SegmentCount.Value >= 0 - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// MyClient.EnableSegmentData(); - /// MyClient.Connect( "localhost" ); - /// Output_GetSegmentCount Output; - /// Output = MyClient.GetSegmentCount( "Bob" ); // Output.Result == NoFrame - /// // Output.SegmentCount == 0 - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentCount( "Al" ); // Output.Result == - /// // InvalidSubjectName - /// // Output.SegmentCount == 0 - /// Output = MyClient.GetSegmentCount( "Bob" );// Output.Result == Success - /// // Output.SegmentCount >= 0 - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// MyClient.Connect( "localhost" ); - /// Output_GetSegmentCount Output; - /// Output = MyClient.GetSegmentCount( "Bob" ); // Output.Result == NoFrame - /// // Output.SegmentCount == 0 - /// MyClient.UpdateFrame(); - /// Output = MyClient.GetSegmentCount( "Al" ); // Output.Result == - /// // InvalidSubjectName - /// // Output.SegmentCount == 0 - /// Output = MyClient.GetSegmentCount( "Bob" );// Output.Result == Success - /// // Output.SegmentCount >= 0 - /// ----- - /// - /// - /// \param SubjectName The name of the subject. - /// \return An Output_GetSegmentCount class containing the result of the operation and the number of segments. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - Output_GetSegmentCount GetSegmentCount(const String & SubjectName) const; - - /// Return the name of a subject segment specified by index. - /// - /// See Also: GetSegmentCount(), GetSegmentChildCount(), GetSegmentChildName(), GetSubjectRootSegmentName() - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// RetimingClient_Connect( pRetimingClient, "localhost" ); - /// COutput_GetSegmentCount SegmentCount; - /// RetimingClient_GetSegmentCount( pRetimingClient, "Bob", &SegmentCount ); - /// // SegmentCount.Result == NOFrame - /// // SegmentCount.Value == 0 - /// RetimingClient_GetFrame( pRetimingClient ); - /// RetimingClient_GetSegmentCount( pRetimingClient, "AI", &SegmentCount ); - /// // SegmentCount.Result == InvalidSubjectName - /// // SegmentCount.Value == 0 - /// RetimingClient_GetSegmentCount( pRetimingClient, "Bob", &SegmentCount ); - /// // SegmentCount.Result == Success - /// // SegmentCount.Value >= 0 - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentName Output; - /// // SegmentIndex must be between 0 and GetSegmentCount() - 1 - /// Output = MyClient.GetSegmentName( "Bob", 0 ); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// MyClient.Connect( "localhost" ); - /// MyClient.UpdateFrame(); - /// Output_GetSegmentName Output; - /// // SegmentIndex must be between 0 and GetSegmentCount() - 1 - /// Output = MyClient.GetSegmentName( "Bob", 0 ); - /// ----- - /// - /// - /// \param SubjectName The name of the subject - /// \param SegmentIndex The index of the segment - /// \return An Output_GetSegmentName class containing the result of the operation and the name of the parent segment or an empty string if it is the root segment. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - /// + InvalidSubjectName - Output_GetSegmentName GetSegmentName(const String & SubjectName, - const unsigned int SegmentIndex) const; - - /// Return the number of child segments for a specified subject segment. This can be passed into segment functions. - /// - /// See Also: GetSegmentCount() - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// RetimingClient_Connect( pRetimingClient, "localhost" ); - /// RetimingClient_GetFrame( pRetimingClient ); - /// COutput_GetSegmentChildCount ChildCount; - /// RetimingClient_GetSegmentChildCount(pRetimingClient, "Bob", "Pelvis", &ChildCount); - /// // ChildCount.Result == Success - /// // ChildCount.SegmentCount == 2 - /// RetimingClient_GetSegmentChildCount(pRetimingClient, "Alice", "Pelvis", &ChildCount); - /// // ChildCount.Result == InvalidSubjectName - /// // ChildCount.SegmentCount == 0 - /// char SegmentName[128]; - /// RetimingClient_GetSegmentName(pRetimingClient, "Bob", , 128, SegmentName); - /// RetimingClient_GetSegmentName(pRetimingClient, "Bob", &SegmentName); - /// // ChildCount.Result == Success - /// // ChildCount.SegmentCount == 2 - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentChildCount OutputGSCC; - /// OutputGSCC = MyClient.GetSegmentChildCount( "Bob", "Pelvis" ); - /// // OutputGSCC.Result == Success - /// // OutputGSCC.SegmentCount == 2 - /// Output_GetSegmentChildName OutputGSCN; - /// OutputGSCN = MyClient.GetSegmentName( "Alice", 0 ); - /// // OutputGSN.Result == InvalidSubjectName - /// // OutputGSN.SegmentName == "" - /// OutputGSCN = MyClient.GetSegmentName( "Bob", "Pelvis", 0 ); - /// // OutputGSCN.Result == Success - /// // OutputGSCN.SegmentName == "LFemur" - /// OutputGSCN = MyClient.GetSegmentName( "Bob", "Pelvis", 1 ); - /// // OutputGSCN.Result == Success - /// // OutputGSCN.SegmentName == "RFemur" - /// OutputGSCN = MyClient.GetSegmentName( "Bob", "Pelvis", 2 ); - /// // OutputGSCN.Result == InvalidIndex - /// // OutputGSCN.SegmentName == "" - /// // (no third segment) - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// MyClient.Connect( "localhost" ); - /// MyClient.UpdateFrame(); - /// Output_GetSegmentChildCount OutputGSCC; - /// OutputGSCC = MyClient.GetSegmentChildCount( "Bob", "Pelvis" ); - /// // OutputGSCC.Result == Success - /// // OutputGSCC.SegmentCount == 2 - /// Output_GetSegmentChildName OutputGSCN; - /// OutputGSCN = MyClient.GetSegmentName( "Alice", 0 ); - /// // OutputGSN.Result == InvalidSubjectName - /// // OutputGSN.SegmentName == "" - /// OutputGSCN = MyClient.GetSegmentName( "Bob", "Pelvis", 0 ); - /// // OutputGSCN.Result == Success - /// // OutputGSCN.SegmentName == "LFemur" - /// OutputGSCN = MyClient.GetSegmentName( "Bob", "Pelvis", 1 ); - /// // OutputGSCN.Result == Success - /// // OutputGSCN.SegmentName == "RFemur" - /// OutputGSCN = MyClient.GetSegmentName( "Bob", "Pelvis", 2 ); - /// // OutputGSCN.Result == InvalidIndex - /// // OutputGSCN.SegmentName == "" - /// // (no third segment) - /// ----- - /// - /// \param SubjectName The name of the subject - /// \param SegmentName The name of the segment - /// \return An Output_GetSegmentChildCount class containing the result of the operation and the number of child segments. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - /// + InvalidSubjectName - /// + InvalidSegmentName - Output_GetSegmentChildCount GetSegmentChildCount(const String & SubjectName, - const String & SegmentName) const; - - /// Return the name of the child segment for a specified subject segment and index. - /// - /// See Also: GetSegmentCount(), GetSegmentChildCount(), GetSegmentChildName(), GetSubjectRootSegmentName() - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// RetimingClient_Connect( pRetimingClient, "localhost" ); - /// RetimingClient_EnableSegmentData( pRetimingClient ); - /// RetimingClient_GetFrame( pRetimingClient ); - /// char SegmentChildName[128]; - /// // Segment index must be between 0 and RetimingClient_GetSegmentChildCount() - 1 - /// RetimingClient_GetSegmentChildName( pRetimingClient, "Bob", "Pelvis", 0, 128, SegmentChildName ); - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentChildName Output; - /// // Segment index must be between 0 and GetSegmentChildCount() - 1 - /// Output = MyClient.GetSegmentChildName( "Bob", "Pelvis", 0 ); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// MyClient.Connect( "localhost" ); - /// MyClient.UpdateFrame(); - /// Output_GetSegmentChildName Output; - /// // Segment index must be between 0 and GetSegmentChildCount() - 1 - /// Output = MyClient.GetSegmentChildName( "Bob", "Pelvis", 0 ); - /// ----- - /// - /// \param SubjectName The name of the subject - /// \param SegmentName The name of the segment - /// \param SegmentIndex The index of the child segment. A valid Segment Index is between 0 and GetSegmentChildCount()-1. - /// \return An Output_GetSegmentChildName class containing the result of the operation and the name of the child segment. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - /// + InvalidSubjectName - /// + InvalidSegmentName - Output_GetSegmentChildName GetSegmentChildName(const String & SubjectName, - const String & SegmentName, - const unsigned int SegmentIndex) const; - - /// Return the name of the parent segment for a specified subject segment. If the specified segment is - /// the root segment of the subject then it will return an empty string. - /// - /// See Also: GetSegmentCount(), GetSegmentChildCount(), GetSegmentChildName(), GetSubjectRootSegmentName() - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// RetimingClient_Connect( pRetimingClient, "localhost" ); - /// RetimingClient_GetFrame( pRetimingClient ); - /// char SegmentParentName[128]; - /// CEnum Result = RetimingClient_GetSegmentParentName( - /// pRetimingClient, "Bob", "Pelvis", 128, SegmentParentName); - /// // Result == Success - /// // SegmentParentName = "" - /// // This is the root segment - /// Result = RetimingClient_GetSegmentParentName(pRetimingClient, "Bob", "LFemur", 128, SegmentParentName); - /// // Result == Success - /// // SegmentParentName = "Pelvis" - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentParentName Output; - /// Output = MyClient.GetSegmentParentName( "Bob", "Pelvis" ); - /// // Output.Result == Success - /// // Output.SegmentName == "" - /// // This is the root segment - /// Output = MyClient.GetSegmentParentName( "Bob", "LFemur" ); - /// // Output.Result == Success - /// // Output.SegmentName == "Pelvis" - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// MyClient.Connect( "localhost" ); - /// MyClient.UpdateFrame(); - /// Output_GetSegmentParentName Output; - /// Output = MyClient.GetSegmentParentName( "Bob", "Pelvis" ); - /// // Output.Result == Success - /// // Output.SegmentName == "" - /// // This is the root segment - /// Output = MyClient.GetSegmentParentName( "Bob", "LFemur" ); - /// // Output.Result == Success - /// // Output.SegmentName == "Pelvis" - /// ----- - /// - /// \param SubjectName The name of the subject - /// \param SegmentName The name of the segment - /// \return An Output_GetSegmentParentName class containing the result of the operation and the name of the parent segment or an empty string if it is the root segment. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - Output_GetSegmentParentName GetSegmentParentName(const String & SubjectName, - const String & SegmentName) const; - - /// Return the static pose translation of a subject segment. - /// - /// See Also: GetSegmentStaticRotationHelical(), GetSegmentStaticRotationMatrix(), GetSegmentStaticRotationQuaternion(), GetSegmentStaticRotationEulerXYZ(), GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ() - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// RetimingClient_Connect( pRetimingClient, "localhost" ); - /// RetimingClient_GetFrame( pRetimingClient ); - /// COutput_GetSegmentStaticTranslation _Output_GetSegmentStaticTranslation; - /// RetimingClient_GetSegmentStaticTranslation( - /// pRetimingClient, "Alice", "Pelvis", &_Output_GetSegmentStaticTranslation); - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentStaticTranslation Output = - /// MyClient.GetSegmentStaticTranslation( "Alice", "Pelvis" ); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// MyClient.Connect( "localhost" ); - /// MyClient.UpdateFrame(); - /// Output_GetSegmentStaticTranslation Output = - /// MyClient.GetSegmentStaticTranslation( "Alice", "Pelvis" ); - /// ----- - /// - /// \param SubjectName The name of the subject - /// \param SegmentName The name of the segment - /// \return An Output_GetSegmentStaticTranslation class containing the result of the operation and the translation of the segment. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - Output_GetSegmentStaticTranslation GetSegmentStaticTranslation(const String & SubjectName, - const String & SegmentName) const; - - /// Return the static pose rotation of a subject segment in helical coordinates. - /// The helical coordinates represent a vector whose length is the amount of rotation in radians, and the direction is the axis about which to rotate. - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// RetimingClient_Connect( pRetimingClient, "localhost" ); - /// RetimingClient_GetFrame( pRetimingClient ); - /// COutput_GetSegmentStaticRotationHelical _Output_GetSegmentStaticRotationHelical; - /// RetimingClient_GetSegmentStaticRotationHelical( - /// pRetimingClient, "Alice", "Pelvis", &_Output_GetSegmentStaticRotationHelical); - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentStaticRotationHelical Output = - /// MyClient.GetSegmentStaticRotationHelical( "Alice", "Pelvis" ); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// MyClient.Connect( "localhost" ); - /// MyClient.UpdateFrame(); - /// Output_GetSegmentStaticRotationHelical Output = - /// MyClient.GetSegmentStaticRotationHelical( "Alice", "Pelvis" ); - /// ----- - /// See Also: GetSegmentStaticTranslation(), GetSegmentStaticRotationMatrix(), GetSegmentStaticRotationQuaternion(), GetSegmentStaticRotationEulerXYZ(), GetSegmentLocalTranslation, GetSegmentLocalRotationHelical, GetSegmentLocalRotationMatrix, GetSegmentLocalRotationQuaternion, GetSegmentLocalRotationEulerXYZ - /// - /// - /// - /// \param SubjectName The name of the subject - /// \param SegmentName The name of the segment - /// \return An Output_GetSegmentStaticRotationHelical class containing the result of the operation and the rotation of the segment. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - Output_GetSegmentStaticRotationHelical GetSegmentStaticRotationHelical(const String & SubjectName, - const String & SegmentName) const; - - /// Return the static pose rotation of a subject segment as a 3x3 row-major matrix. - /// - /// See Also: GetSegmentStaticTranslation(), GetSegmentStaticRotationHelical(), GetSegmentStaticRotationQuaternion(), GetSegmentStaticRotationEulerXYZ(), GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ() - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// RetimingClient_Connect( pRetimingClient, "localhost" ); - /// RetimingClient_GetFrame( pRetimingClient ); - /// COutput_GetSegmentStaticRotationMatrix _Output_GetSegmentStaticRotationMatrix; - /// RetimingClient_GetSegmentStaticRotationMatrix( - /// pRetimingClient, "Alice", "Pelvis", &_Output_GetSegmentStaticRotationMatrix); - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentStaticRotationMatrix Output = - /// MyClient.GetSegmentStaticRotationMatrix( "Alice", "Pelvis" ); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// MyClient.Connect( "localhost" ); - /// MyClient.UpdateFrame(); - /// Output_GetSegmentStaticRotationMatrix Output = - /// MyClient.GetSegmentStaticRotationMatrix( "Alice", "Pelvis" ); - /// ----- - /// - /// - /// \param SubjectName The name of the subject - /// \param SegmentName The name of the segment - /// \return An Output_GetSegmentStaticRotationMatrix class containing the result of the operation and the rotation of the segment. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - Output_GetSegmentStaticRotationMatrix GetSegmentStaticRotationMatrix(const String & SubjectName, - const String & SegmentName) const; - - /// Return the static pose rotation of a subject segment in quaternion coordinates. - /// The quaternion is of the form (x, y, z, w) where w is the real component and x, y and z are the imaginary components. - /// N.B. This is different from that used in many other applications, which use (w, x, y, z). - /// - /// See Also: GetSegmentStaticTranslation(), GetSegmentStaticRotationHelical(), GetSegmentStaticRotationMatrix(), GetSegmentStaticRotationEulerXYZ(), GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ() - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// RetimingClient_Connect( pRetimingClient, "localhost" ); - /// RetimingClient_GetFrame( pRetimingClient ); - /// COutput_GetSegmentStaticRotationQuaternion _Output_GetSegmentStaticRotationQuaternion; - /// RetimingClient_GetSegmentStaticRotationQuaternion( - /// pRetimingClient, "Alice", "Pelvis", &_Output_GetSegmentStaticRotationQuaternion); - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentStaticRotationQuaternion Output = - /// MyClient.GetSegmentStaticRotationQuaternion( "Alice", "Pelvis" ); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// MyClient.Connect( "localhost" ); - /// MyClient.UpdateFrame(); - /// Output_GetSegmentStaticRotationQuaternion Output = - /// MyClient.GetSegmentStaticRotationQuaternion( "Alice", "Pelvis" ); - /// ----- - /// - /// - /// - /// \param SubjectName The name of the subject - /// \param SegmentName The name of the segment - /// \return An Output_GetSegmentStaticRotationQuaternion class containing the result of the operation and the rotation of the segment. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - Output_GetSegmentStaticRotationQuaternion GetSegmentStaticRotationQuaternion(const String & SubjectName, - const String & SegmentName) const; - - /// Return the static pose rotation of a subject segment in Euler XYZ coordinates. - /// - /// See Also: GetSegmentStaticTranslation(), GetSegmentStaticRotationHelical(), GetSegmentStaticRotationMatrix(), - /// GetSegmentStaticRotationQuaternion(), GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), - /// GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ(). - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// RetimingClient_Connect( pRetimingClient, "localhost" ); - /// RetimingClient_GetFrame( pRetimingClient ); - /// COutput_GetSegmentStaticRotationEulerXYZ _Output_GetSegmentStaticRotationEulerXYZ; - /// RetimingClient_GetSegmentStaticRotationEulerXYZ( - /// pRetimingClient, "Alice", "Pelvis", &_Output_GetSegmentStaticRotationEulerXYZ); - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentStaticRotationEulerXYZ Output; - /// Output = MyClient.GetSegmentStaticRotationEulerXYZ( "Alice", "Pelvis" ); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// MyClient.Connect( "localhost" ); - /// MyClient.UpdateFrame(); - /// Output_GetSegmentStaticRotationEulerXYZ Output; - /// Output = MyClient.GetSegmentStaticRotationEulerXYZ( "Alice", "Pelvis" ); - /// ----- - /// - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentStaticRotationEulerXYZ class containing the result of the request - /// and the rotation of the segment \f$(x,y,z)\f$. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - Output_GetSegmentStaticRotationEulerXYZ GetSegmentStaticRotationEulerXYZ(const String & SubjectName, - const String & SegmentName) const; - - /// Return the translation of a subject segment in global coordinates. - /// The translation is of the form ( x, y, z ) where x, y and z are in millimeters with respect to the global origin. - /// - /// See Also: GetSegmentGlobalRotationHelical(), GetSegmentGlobalRotationMatrix(), GetSegmentGlobalRotationQuaternion(), GetSegmentGlobalRotationEulerXYZ(), GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ() - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// RetimingClient_Connect( pRetimingClient, "localhost" ); - /// RetimingClient_GetFrame( pRetimingClient ); - /// COutput_GetSegmentGlobalTranslation _Output_GetSegmentGlobalTranslation; - /// RetimingClient_GetSegmentGlobalTranslation( - /// pRetimingClient, "Alice", "Pelvis", &_Output_GetSegmentGlobalTranslation); - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentGlobalTranslation Output = - /// MyClient.GetSegmentGlobalTranslation( "Alice", "Pelvis" ); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// MyClient.Connect( "localhost" ); - /// MyClient.UpdateFrame(); - /// Output_GetSegmentGlobalTranslation Output = - /// MyClient.GetSegmentGlobalTranslation( "Alice", "Pelvis" ); - /// ----- - /// - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentGlobalTranslation class containing the result of the operation, the translation of the segment, and whether the segment is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - /// - Occluded will be True if the segment was absent at this frame. In this case the translation will be [0,0,0]. - Output_GetSegmentGlobalTranslation GetSegmentGlobalTranslation(const String & SubjectName, - const String & SegmentName) const; - - /// Return the rotation of a subject segment in global helical coordinates. - /// - /// See Also: GetSegmentGlobalTranslation(), GetSegmentGlobalRotationMatrix(), GetSegmentGlobalRotationQuaternion(), GetSegmentGlobalRotationEulerXYZ(), GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ() - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// RetimingClient_Connect( pRetimingClient, "localhost" ); - /// RetimingClient_GetFrame( pRetimingClient ); - /// COutput_GetSegmentGlobalRotationHelical _Output_GetSegmentGlobalRotationHelical; - /// RetimingClient_GetSegmentGlobalRotationHelical( - /// pRetimingClient, "Alice", "Pelvis", &_Output_GetSegmentGlobalRotationHelical); - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentGlobalRotationHelical Output = - /// MyClient.GetSegmentGlobalRotationHelical( "Alice", "Pelvis" ); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// MyClient.Connect( "localhost" ); - /// MyClient.UpdateFrame(); - /// Output_GetSegmentGlobalRotationHelical Output = - /// MyClient.GetSegmentGlobalRotationHelical( "Alice", "Pelvis" ); - /// ----- - /// - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentGlobalRotationHelical class containing the result of the operation, the rotation of the segment, and whether the segment is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - /// - Occluded will be True if the segment was absent at this frame. In this case, the rotation will be [0,0,0]. - Output_GetSegmentGlobalRotationHelical GetSegmentGlobalRotationHelical(const String & SubjectName, - const String & SegmentName) const; - - /// Return the rotation of a subject segment as a 3x3 row-major matrix in global coordinates. - /// - /// See Also: GetSegmentGlobalTranslation(), GetSegmentGlobalRotationHelical(), GetSegmentGlobalRotationQuaternion(), GetSegmentGlobalRotationEulerXYZ(), GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ() - /// - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// RetimingClient_Connect( pRetimingClient, "localhost" ); - /// RetimingClient_GetFrame( pRetimingClient ); - /// COutput_GetSegmentGlobalRotationMatrix _Output_GetSegmentGlobalRotationMatrix; - /// RetimingClient_GetSegmentGlobalRotationMatrix( - /// pRetimingClient, "Alice", "Pelvis", &_Output_GetSegmentGlobalRotationMatrix); - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentGlobalRotationMatrix Output = - /// MyClient.GetSegmentGlobalRotationMatrix( "Alice", "Pelvis" ); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// MyClient.Connect( "localhost" ); - /// MyClient.UpdateFrame(); - /// Output_GetSegmentGlobalRotationMatrix Output = - /// MyClient.GetSegmentGlobalRotationMatrix( "Alice", "Pelvis" ); - /// ----- - /// - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentGlobalRotationMatrix Class containing the result of the operation, the rotation of the segment, and whether the segment is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - /// - Occluded will be True if the segment was absent at this frame. - Output_GetSegmentGlobalRotationMatrix GetSegmentGlobalRotationMatrix(const String & SubjectName, - const String & SegmentName) const; - - /// Return the rotation of a subject segment in global quaternion coordinates. - /// The quaternion is of the form (x, y, z, w) where w is the real component and x, y and z are the imaginary components. - /// N.B. This is different from that used in many other applications, which use (w, x, y, z). - /// - /// See Also: GetSegmentGlobalTranslation(), GetSegmentGlobalRotationHelical(), GetSegmentGlobalRotationMatrix(), GetSegmentGlobalRotationEulerXYZ(), GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ() - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// RetimingClient_Connect( pRetimingClient, "localhost" ); - /// RetimingClient_GetFrame( pRetimingClient ); - /// COutput_GetSegmentGlobalRotationQuaternion _Output_GetSegmentGlobalRotationQuaternion; - /// RetimingClient_GetSegmentGlobalRotationQuaternion( - /// pRetimingClient, "Alice", "Pelvis", &_Output_GetSegmentGlobalRotationQuaternion); - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentGlobalRotationQuaternion Output = - /// MyClient.GetSegmentGlobalRotationQuaternion( "Alice", "Pelvis" ); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// MyClient.Connect( "localhost" ); - /// MyClient.UpdateFrame(); - /// Output_GetSegmentGlobalRotationQuaternion Output = - /// MyClient.GetSegmentGlobalRotationQuaternion( "Alice", "Pelvis" ); - /// ----- - /// - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentGlobalRotationQuaternion class containing the result of the operation, the rotation of the segment, and whether the segment is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - /// - Occluded will be True if the segment was absent at this frame. In this case the Rotation will be [1,0,0,0]. - Output_GetSegmentGlobalRotationQuaternion GetSegmentGlobalRotationQuaternion(const String & SubjectName, - const String & SegmentName) const; - - /// Return the rotation of a subject segment in global Euler XYZ coordinates. - /// - /// See Also: GetSegmentGlobalTranslation(), GetSegmentGlobalRotationHelical(), GetSegmentGlobalRotationMatrix(), GetSegmentGlobalRotationQuaternion(), GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ() - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// RetimingClient_Connect( pRetimingClient, "localhost" ); - /// RetimingClient_GetFrame( pRetimingClient ); - /// COutput_GetSegmentGlobalRotationEulerXYZ _Output_GetSegmentGlobalRotationEulerXYZ; - /// RetimingClient_GetSegmentGlobalRotationEulerXYZ( - /// pRetimingClient, "Alice", "Pelvis", &_Output_GetSegmentGlobalRotationEulerXYZ); - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentGlobalRotationEulerXYZ Output = - /// MyClient.GetSegmentGlobalRotationEulerXYZ( "Alice", "Pelvis" ); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// MyClient.Connect( "localhost" ); - /// MyClient.UpdateFrame(); - /// Output_GetSegmentGlobalRotationEulerXYZ Output = - /// MyClient.GetSegmentGlobalRotationEulerXYZ( "Alice", "Pelvis" ); - /// ----- - /// - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentGlobalRotationEulerXYZ class containing the result of the operation, the rotation of the segment, and whether the segment is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - /// - Occluded will be True if the segment was absent at this frame. In this case the rotation will be [0,0,0]. - Output_GetSegmentGlobalRotationEulerXYZ GetSegmentGlobalRotationEulerXYZ(const String & SubjectName, - const String & SegmentName) const; - - /// Return the translation of a subject segment in local coordinates relative to its parent segment. - /// - /// See Also: GetSegmentLocalRotationHelical(), GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ(), GetSegmentGlobalTranslation(), GetSegmentGlobalRotationHelical(), GetSegmentGlobalRotationMatrix(), GetSegmentGlobalRotationQuaternion(), GetSegmentGlobalRotationEulerXYZ() - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// RetimingClient_Connect( pRetimingClient, "localhost" ); - /// RetimingClient_GetFrame( pRetimingClient ); - /// COutput_GetSegmentLocalTranslation _Output_GetSegmentLocalTranslation; - /// RetimingClient_GetSegmentLocalTranslation( - /// pRetimingClient, "Alice", "Pelvis", &_Output_GetSegmentLocalTranslation); - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentLocalTranslation Output = - /// MyClient.GetSegmentLocalTranslation( "Alice", "Pelvis" ); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// MyClient.Connect( "localhost" ); - /// MyClient.UpdateFrame(); - /// Output_GetSegmentLocalTranslation Output = - /// MyClient.GetSegmentLocalTranslation( "Alice", "Pelvis" ); - /// ----- - /// - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentLocalTranslation class containing the result of the operation, the translation of the segment, and whether the segment is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - /// - Occluded will be True if the segment was absent at this frame. In this case the translation will be [0,0,0]. - Output_GetSegmentLocalTranslation GetSegmentLocalTranslation(const String & SubjectName, - const String & SegmentName) const; - - /// Return the rotation of a subject segment in local helical coordinates relative to its parent segment. - /// - /// See Also: GetSegmentLocalTranslation(), GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ(), GetSegmentGlobalTranslation(), GetSegmentGlobalRotationHelical(), GetSegmentGlobalRotationMatrix(), GetSegmentGlobalRotationQuaternion(), GetSegmentGlobalRotationEulerXYZ() - /// - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// RetimingClient_Connect( pRetimingClient, "localhost" ); - /// RetimingClient_GetFrame( pRetimingClient ); - /// COutput_GetSegmentLocalRotationHelical _Output_GetSegmentLocalRotationHelical; - /// RetimingClient_GetSegmentLocalRotationHelical( - /// pRetimingClient, "Alice", "Pelvis", &_Output_GetSegmentLocalRotationHelical); - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentLocalRotationHelical Output = - /// MyClient.GetSegmentLocalRotationHelical( "Alice", "Pelvis" ); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// MyClient.Connect( "localhost" ); - /// MyClient.UpdateFrame(); - /// Output_GetSegmentLocalRotationHelical Output = - /// MyClient.GetSegmentLocalRotationHelical( "Alice", "Pelvis" ); - /// ----- - /// - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentLocalRotationHelical class containing the result of the operation, the rotation of the segment, and whether the segment is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - /// - Occluded will be True if the segment was absent at this frame. In this case the Rotation will be [0,0,0]. - Output_GetSegmentLocalRotationHelical GetSegmentLocalRotationHelical(const String & SubjectName, - const String & SegmentName) const; - - /// Return the rotation row-major matrix of a subject segment in local coordinates relative to its parent segment. - /// - /// See Also: GetSegmentLocalTranslation(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ(), GetSegmentGlobalTranslation(), GetSegmentGlobalRotationHelical(), GetSegmentGlobalRotationMatrix() , GetSegmentGlobalRotationQuaternion(), GetSegmentGlobalRotationEulerXYZ() - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// RetimingClient_Connect( pRetimingClient, "localhost" ); - /// RetimingClient_GetFrame( pRetimingClient ); - /// COutput_GetSegmentLocalRotationMatrix _Output_GetSegmentLocalRotationMatrix; - /// RetimingClient_GetSegmentLocalRotationMatrix( - /// pRetimingClient, "Alice", "Pelvis", &_Output_GetSegmentLocalRotationMatrix); - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentLocalRotationMatrix Output = - /// MyClient.GetSegmentLocalRotationMatrix( "Alice", "Pelvis" ); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// MyClient.Connect( "localhost" ); - /// MyClient.UpdateFrame(); - /// Output_GetSegmentLocalRotationMatrix Output = - /// MyClient.GetSegmentLocalRotationMatrix( "Alice", "Pelvis" ); - /// ----- - /// - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentLocalRotationMatrix class containing the result of the operation, the rotation of the segment, and whether the segment is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - /// - Occluded will be True if the segment was absent at this frame. - Output_GetSegmentLocalRotationMatrix GetSegmentLocalRotationMatrix(const String & SubjectName, - const String & SegmentName) const; - - /// Return the rotation of a subject segment in local quaternion coordinates relative to its parent segment. - /// The quaternion is of the form (x, y, z, w) where w is the real component and x, y and z are the imaginary components. N.B. This is different from that used in many other applications, which use (w, x, y, z). - /// - /// See Also: GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationEulerXYZ(), GetSegmentGlobalTranslation(), GetSegmentGlobalRotationHelical(), GetSegmentGlobalRotationMatrix(), GetSegmentGlobalRotationQuaternion(), GetSegmentGlobalRotationEulerXYZ() - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// RetimingClient_Connect( pRetimingClient, "localhost" ); - /// RetimingClient_GetFrame( pRetimingClient ); - /// COutput_GetSegmentLocalRotationQuaternion _Output_GetSegmentLocalRotationQuaternion; - /// RetimingClient_GetSegmentLocalRotationQuaternion( - /// pRetimingClient, "Alice", "Pelvis", &_Output_GetSegmentLocalRotationQuaternion); - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentLocalRotationQuaternion Output = - /// MyClient.GetSegmentLocalRotationQuaternion( "Alice", "Pelvis" ); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// MyClient.Connect( "localhost" ); - /// MyClient.UpdateFrame(); - /// Output_GetSegmentLocalRotationQuaternion Output = - /// MyClient.GetSegmentLocalRotationQuaternion( "Alice", "Pelvis" ); - /// ----- - /// - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentLocalRotationQuaternion class containing the result of the operation, the rotation of the segment, and whether the segment is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - /// - Occluded will be True if the segment was absent at this frame. In this case the rotation will be [1,0,0,0]. - Output_GetSegmentLocalRotationQuaternion GetSegmentLocalRotationQuaternion(const String & SubjectName, - const String & SegmentName) const; - - /// Return the rotation of a subject segment in local Euler XYZ coordinates relative to its parent segment. - /// - /// See Also: GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationQuaternion(), GetSegmentGlobalTranslation(), GetSegmentGlobalRotationHelical(), GetSegmentGlobalRotationMatrix() , GetSegmentGlobalRotationQuaternion(), GetSegmentGlobalRotationEulerXYZ() - /// - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// RetimingClient_Connect( pRetimingClient, "localhost" ); - /// RetimingClient_GetFrame( pRetimingClient ); - /// COutput_GetSegmentLocalRotationEulerXYZ _Output_GetSegmentLocalRotationEulerXYZ; - /// RetimingClient_GetSegmentLocalRotationEulerXYZ( - /// pRetimingClient, "Alice", "Pelvis", &_Output_GetSegmentLocalRotationEulerXYZ); - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentLocalRotationEulerXYZ Output = - /// MyClient.GetSegmentLocalRotationEulerXYZ( "Alice", "Pelvis" ); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// MyClient.Connect( "localhost" ); - /// MyClient.UpdateFrame(); - /// Output_GetSegmentLocalRotationEulerXYZ Output = - /// MyClient.GetSegmentLocalRotationEulerXYZ( "Alice", "Pelvis" ); - /// ----- - /// - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentLocalRotationEulerXYZ class containing the result of the operation, the rotation of the segment, and whether the segment is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - /// - Occluded will be True if the segment was absent at this frame. In this case the rotation will be [0,0,0]. - Output_GetSegmentLocalRotationEulerXYZ GetSegmentLocalRotationEulerXYZ(const String & SubjectName, - const String & SegmentName) const; - - - ///@private - void SetOutputLatency(double OutputLatency); - - ///@private - double OutputLatency() const; - - /// Sets the maximum amount by which the interpolation engine will predict later than the latest received frame. If required to predict by more than this amount, - /// the result LateDataRequested will be returned. - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// RetimingClient_SetMaximumPrediction( pRetimingClient, 30 ); - /// RetimingClient_Connect( pRetimingClient, "localhost" ); - /// RetimingClient_GetFrame( pRetimingClient ); - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// MyClient.SetMaximumPrediction( 30 ); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// MyClient.SetMaximumPrediction( 30 ); - /// MyClient.Connect( "localhost" ); - /// MyClient.UpdateFrame(); - /// ----- - /// \param MaxPrediction The maximum amount of prediction required in milliseconds - void SetMaximumPrediction(double MaxPrediction); - - /// Returns the maximum prediction value currently in use. The default value is 100 ms. - /// - /// - /// C example - /// - /// CRetimingClient * pRetimingClient = RetimingClient_Create(); - /// RetimingClient_SetMaximumPrediction( pRetimingClient, 30 ); - /// RetimingClient_MaximumPrediction( pRetimingClient ); // Returns 30 - /// RetimingClient_Destroy( pRetimingClient ); - /// - /// C++ example - /// - /// ViconDataStreamSDK::CPP::RetimingClient MyClient; - /// MyClient.SetMaximumPrediction( 30 ); - /// MyClient.MaximumPrediction(); // Returns 30 - /// - /// MATLAB example - /// - /// Not implemented - /// - /// .NET example - /// - /// ViconDataStreamSDK.DotNET.RetimingClient MyClient = new ViconDataStreamSDK.DotNET.RetimingClient(); - /// MyClient.SetMaximumPrediction( 30 ); - /// MyClient.MaximumPrediction(); // Returns 30 - /// ----- - /// \return The maximum prediction allowed in milliseconds - double MaximumPrediction() const; - - private: - RetimingClientImpl * m_pClientImpl; - }; - } -} diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/IDataStreamClientBase.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/IDataStreamClientBase.h deleted file mode 100644 index dbe052e75..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/IDataStreamClientBase.h +++ /dev/null @@ -1,1947 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// The SDK client and supporting definitions. - -#ifdef WIN32 - -#ifdef _EXPORTING - #define CLASS_DECLSPEC __declspec(dllexport) -#else - #define CLASS_DECLSPEC __declspec(dllimport) -#endif // _EXPORTING - -#elif defined( __GNUC__ ) - -#if __GNUC__ < 4 - #error gcc 4 is required. - #endif - #define CLASS_DECLSPEC __attribute__((visibility("default"))) - -#else - -#define CLASS_DECLSPEC - -#endif - -#include -#include - -namespace ViconDataStreamSDK -{ -namespace CPP -{ - -class IStringFactory -{ -public: - virtual char * AllocAndCopyString( const char * i_pSource ) = 0; - virtual void FreeString( char * i_pString ) = 0; -protected: - virtual ~IStringFactory() {} -}; - -// A string-managing class for DLL boundary safe text. -// It ensures that strings are de-allocated with the same -// memory manager they are made with. This allows you to use a debug -// build of your client with a release SDK. -class String -{ -public: - // A string which we are not responsible for deallocating - inline - String( const char * i_pString = 0 ) - : m_pString( 0 ) - , m_pConstString( i_pString ) - , m_pStringFactory( 0 ) - { - } - - // A string which we are not responsible for deallocating - String( const std::string & i_rString ) - : m_pString( 0 ) - , m_pConstString( i_rString.c_str() ) - , m_pStringFactory( 0 ) - { - } - - // Copy constructor - inline - String( const String & i_rString ) - { - m_pConstString = i_rString.m_pConstString; - m_pStringFactory = i_rString.m_pStringFactory; - if( m_pStringFactory ) - { - m_pString = m_pStringFactory->AllocAndCopyString( i_rString.m_pString ); - } - else - { - m_pString = 0; - } - } - - inline - String & operator=( const String & i_rString ) - { - m_pConstString = i_rString.m_pConstString; - m_pStringFactory = i_rString.m_pStringFactory; - if ( m_pStringFactory ) - { - m_pString = m_pStringFactory->AllocAndCopyString( i_rString.m_pString ); - } - else - { - m_pString = 0; - } - return *this; - } - - inline - ~String() - { - if( m_pStringFactory ) - { - m_pStringFactory->FreeString( m_pString ); - } - } - - // A string which we are responsible for deallocating - inline - void Set( const char * i_pString, IStringFactory & i_rStringFactory ) - { - m_pString = i_rStringFactory.AllocAndCopyString( i_pString ); - m_pStringFactory = &i_rStringFactory; - m_pConstString = 0; - } - - inline - operator std::string() const - { - if( m_pStringFactory ) - { - return std::string( m_pString ); - } - else - { - return std::string( m_pConstString ); - } - } - -private: - char * m_pString; - const char * m_pConstString; - IStringFactory * m_pStringFactory; -}; - -// Streaming operator for String -inline std::ostream & operator<<( std::ostream & io_rStream, const String & i_rString ) -{ - io_rStream << std::string( i_rString ); - return io_rStream; -} - -namespace Direction -{ - enum Enum - { - Up, - Down, - Left, - Right, - Forward, - Backward - }; -} - -namespace ServerOrientation -{ - enum Enum - { - Unknown, - YUp, - ZUp, - }; -} - -namespace StreamMode -{ - enum Enum - { - ClientPull, - ClientPullPreFetch, - ServerPush - }; -} - -namespace TimecodeStandard -{ - enum Enum - { - None, - PAL, - NTSC, - NTSCDrop, - Film, - NTSCFilm, - ATSC - }; -} - -namespace DeviceType -{ - enum Enum - { - Unknown, - ForcePlate, - EyeTracker - }; -} - -namespace Unit -{ - enum Enum - { - Unknown, - Volt, - Newton, - NewtonMeter, - Meter, - Kilogram, - Second, - Ampere, - Kelvin, - Mole, - Candela, - Radian, - Steradian, - MeterSquared, - MeterCubed, - MeterPerSecond, - MeterPerSecondSquared, - RadianPerSecond, - RadianPerSecondSquared, - Hertz, - Joule, - Watt, - Pascal, - Lumen, - Lux, - Coulomb, - Ohm, - Farad, - Weber, - Tesla, - Henry, - Siemens, - Becquerel, - Gray, - Sievert, - Katal - }; - -} - -namespace Result -{ - /// The Result code indicates the success or failure of a function. - enum Enum - { - Unknown, ///< The result is unknown. Treat it as a failure. - NotImplemented, ///< The function called has not been implemented in this version of the SDK. - Success, ///< The function call succeeded. - InvalidHostName, ///< The "HostName" parameter passed to Connect() is invalid. - InvalidMulticastIP, ///< The "MulticastIP" parameter was not in the range "224.0.0.0" - "239.255.255.255" - ClientAlreadyConnected, ///< Connect() was called whilst already connected to a DataStream. - ClientConnectionFailed, ///< Connect() could not establish a connection to the DataStream server. - ServerAlreadyTransmittingMulticast, ///< StartTransmittingMulticast() was called when the current DataStream server was already transmitting multicast on behalf of this client. - ServerNotTransmittingMulticast, ///< StopTransmittingMulticast() was called when the current DataStream server was not transmitting multicast on behalf of this client. - NotConnected, ///< You have called a function which requires a connection to the DataStream server, but do not have a connection. - NoFrame, ///< You have called a function which requires a frame to be fetched from the DataStream server, but do not have a frame. - InvalidIndex, ///< An index you have passed to a function is out of range. - InvalidCameraName, ///< The Camera Name you passed to a function is invalid in this frame. - InvalidSubjectName, ///< The Subject Name you passed to a function is invalid in this frame. - InvalidSegmentName, ///< The Segment Name you passed to a function is invalid in this frame. - InvalidMarkerName, ///< The Marker Name you passed to a function is invalid in this frame. - InvalidDeviceName, ///< The Device Name you passed to a function is invalid in this frame. - InvalidDeviceOutputName, ///< The Device Output Name you passed to a function is invalid in this frame. - InvalidLatencySampleName, ///< The Latency Sample Name you passed to a function is invalid in this frame. - CoLinearAxes, ///< The directions passed to SetAxisMapping() contain input which would cause two or more axes to lie along the same line, e.g. "Up" and "Down" are on the same line. - LeftHandedAxes, ///< The directions passed to SetAxisMapping() would result in a left-handed coordinate system. This is not supported in the SDK. - HapticAlreadySet, ///< Haptic feedback is already set. - EarlyDataRequested, ///< Re-timed data requested is from before the first time sample we still have - LateDataRequested, ///< Re-timed data requested is too far into the future to be predicted - InvalidOperation ///< The method called is not valid in the current mode of operation - }; -} - - /// The version information returned by Client::GetVersion. - class Output_GetVersion - { - public: - /// Major version number of the SDK. - unsigned int Major; - /// Minor version number of the SDK. - unsigned int Minor; - /// Point version number of the SDK. - unsigned int Point; - }; - - class Output_SimpleResult - { - public: - Result::Enum Result; - }; - - // Output objects that only return a result enum - class Output_Connect : public Output_SimpleResult {}; - class Output_ConnectToMulticast : public Output_SimpleResult {}; - class Output_Disconnect : public Output_SimpleResult {}; - class Output_StartTransmittingMulticast : public Output_SimpleResult {}; - class Output_StopTransmittingMulticast : public Output_SimpleResult {}; - class Output_EnableSegmentData : public Output_SimpleResult {}; - class Output_EnableMarkerData : public Output_SimpleResult {}; - class Output_EnableUnlabeledMarkerData : public Output_SimpleResult {}; - class Output_EnableMarkerRayData : public Output_SimpleResult {}; - class Output_EnableDeviceData : public Output_SimpleResult {}; - class Output_EnableCentroidData : public Output_SimpleResult {}; - class Output_EnableGreyscaleData : public Output_SimpleResult {}; - class Output_EnableDebugData : public Output_SimpleResult {}; - class Output_DisableSegmentData : public Output_SimpleResult {}; - class Output_DisableMarkerData : public Output_SimpleResult {}; - class Output_DisableUnlabeledMarkerData : public Output_SimpleResult {}; - class Output_DisableMarkerRayData : public Output_SimpleResult {}; - class Output_DisableDeviceData : public Output_SimpleResult {}; - class Output_DisableCentroidData : public Output_SimpleResult {}; - class Output_DisableGreyscaleData : public Output_SimpleResult {}; - class Output_DisableDebugData : public Output_SimpleResult {}; - class Output_SetStreamMode : public Output_SimpleResult {}; - class Output_SetApexDeviceFeedback : public Output_SimpleResult {}; - class Output_SetAxisMapping : public Output_SimpleResult {}; - class Output_GetFrame : public Output_SimpleResult {}; - class Output_UpdateFrame : public Output_SimpleResult {}; - class Output_WaitForFrame : public Output_SimpleResult {}; - class Output_SetCameraFilter : public Output_SimpleResult {}; - - class Output_EnabledFlag - { - public: - bool Enabled; - }; - - // Output objects that only return an enabled flag - class Output_IsSegmentDataEnabled : public Output_EnabledFlag {}; - class Output_IsMarkerDataEnabled : public Output_EnabledFlag {}; - class Output_IsUnlabeledMarkerDataEnabled : public Output_EnabledFlag {}; - class Output_IsMarkerRayDataEnabled : public Output_EnabledFlag {}; - class Output_IsDeviceDataEnabled : public Output_EnabledFlag {}; - class Output_IsCentroidDataEnabled : public Output_EnabledFlag {}; - class Output_IsGreyscaleDataEnabled : public Output_EnabledFlag {}; - class Output_IsDebugDataEnabled : public Output_EnabledFlag {}; - - class Output_IsConnected - { - public: - bool Connected; - }; - - class Output_GetAxisMapping - { - public: - Direction::Enum XAxis; - Direction::Enum YAxis; - Direction::Enum ZAxis; - }; - - class Output_GetServerOrientation - { - public: - Result::Enum Result; - ServerOrientation::Enum Orientation; - }; - - class Output_GetFrameNumber - { - public: - Result::Enum Result; - unsigned int FrameNumber; - }; - - class Output_GetTimecode - { - public: - Result::Enum Result; - unsigned int Hours; - unsigned int Minutes; - unsigned int Seconds; - unsigned int Frames; - unsigned int SubFrame; - bool FieldFlag; - TimecodeStandard::Enum Standard; - unsigned int SubFramesPerFrame; - unsigned int UserBits; - }; - - class Output_GetFrameRate - { - public: - Result::Enum Result; - double FrameRateHz; - }; - - class Output_GetLatencySampleCount - { - public: - Result::Enum Result; - unsigned int Count; - }; - - class Output_GetLatencySampleName - { - public: - Result::Enum Result; - String Name; - }; - - class Output_GetLatencySampleValue - { - public: - Result::Enum Result; - double Value; - }; - - class Output_GetLatencyTotal - { - public: - Result::Enum Result; - double Total; - }; - - class Output_GetFrameRateCount - { - public: - Result::Enum Result; - unsigned int Count; - }; - - class Output_GetFrameRateName - { - public: - Result::Enum Result; - String Name; - }; - - class Output_GetFrameRateValue - { - public: - Result::Enum Result; - double Value; - }; - - class Output_GetHardwareFrameNumber - { - public: - Result::Enum Result; - unsigned int HardwareFrameNumber; - }; - - - class Output_GetSubjectCount - { - public: - Result::Enum Result; - unsigned int SubjectCount; - }; - - class Output_GetSubjectName - { - public: - Result::Enum Result; - String SubjectName; - }; - - class Output_GetSubjectRootSegmentName - { - public: - Result::Enum Result; - String SegmentName; - }; - - class Output_GetSegmentChildCount - { - public: - Result::Enum Result; - unsigned int SegmentCount; - }; - - class Output_GetSegmentChildName - { - public: - Result::Enum Result; - String SegmentName; - }; - - class Output_GetSegmentParentName - { - public: - Result::Enum Result; - String SegmentName; - }; - - class Output_GetSegmentCount - { - public: - Result::Enum Result; - unsigned int SegmentCount; - }; - - class Output_GetSegmentName - { - public: - Result::Enum Result; - String SegmentName; - }; - - class Output_GetSegmentStaticTranslation - { - public: - Result::Enum Result; - double Translation[ 3 ]; - }; - - class Output_GetSegmentStaticRotationHelical - { - public: - Result::Enum Result; - double Rotation[ 3 ]; - }; - - class Output_GetSegmentStaticRotationMatrix - { - public: - Result::Enum Result; - double Rotation[ 9 ]; - }; - - class Output_GetSegmentStaticRotationQuaternion - { - public: - Result::Enum Result; - double Rotation[ 4 ]; - }; - - class Output_GetSegmentStaticRotationEulerXYZ - { - public: - Result::Enum Result; - double Rotation[ 3 ]; - }; - - class Output_GetSegmentGlobalTranslation - { - public: - Result::Enum Result; - double Translation[ 3 ]; - bool Occluded; - }; - - class Output_GetSegmentGlobalRotationHelical - { - public: - Result::Enum Result; - double Rotation[ 3 ]; - bool Occluded; - }; - - class Output_GetSegmentGlobalRotationMatrix - { - public: - Result::Enum Result; - double Rotation[ 9 ]; - bool Occluded; - }; - - class Output_GetSegmentGlobalRotationQuaternion - { - public: - Result::Enum Result; - double Rotation[ 4 ]; - bool Occluded; - }; - - class Output_GetSegmentGlobalRotationEulerXYZ - { - public: - Result::Enum Result; - double Rotation[ 3 ]; - bool Occluded; - }; - - class Output_GetSegmentLocalTranslation - { - public: - Result::Enum Result; - double Translation[ 3 ]; - bool Occluded; - }; - - class Output_GetSegmentLocalRotationHelical - { - public: - Result::Enum Result; - double Rotation[ 3 ]; - bool Occluded; - }; - - class Output_GetSegmentLocalRotationMatrix - { - public: - Result::Enum Result; - double Rotation[ 9 ]; - bool Occluded; - }; - - class Output_GetSegmentLocalRotationQuaternion - { - public: - Result::Enum Result; - double Rotation[ 4 ]; - bool Occluded; - }; - - class Output_GetSegmentLocalRotationEulerXYZ - { - public: - Result::Enum Result; - double Rotation[ 3 ]; - bool Occluded; - }; - - class Output_GetObjectQuality - { - public: - Result::Enum Result; - double Quality; - }; - - class Output_GetMarkerCount - { - public: - Result::Enum Result; - unsigned int MarkerCount; - }; - - class Output_GetMarkerName - { - public: - Result::Enum Result; - String MarkerName; - }; - - class Output_GetMarkerParentName - { - public: - Result::Enum Result; - String SegmentName; - }; - - class Output_GetMarkerGlobalTranslation - { - public: - Result::Enum Result; - double Translation[ 3 ]; - bool Occluded; - }; - - class Output_GetMarkerRayContributionCount - { - public: - Result::Enum Result; - unsigned int RayContributionsCount; - }; - - class Output_GetMarkerRayContribution - { - public: - Result::Enum Result; - unsigned int CameraID; - unsigned int CentroidIndex; - }; - - class Output_GetUnlabeledMarkerCount - { - public: - Result::Enum Result; - unsigned int MarkerCount; - }; - - class Output_GetUnlabeledMarkerGlobalTranslation - { - public: - Result::Enum Result; - double Translation[ 3 ]; - }; - - class Output_GetLabeledMarkerCount - { - public: - Result::Enum Result; - unsigned int MarkerCount; - }; - - class Output_GetLabeledMarkerGlobalTranslation - { - public: - Result::Enum Result; - double Translation[ 3 ]; - }; - - class Output_GetDeviceCount - { - public: - Result::Enum Result; - unsigned int DeviceCount; - }; - - class Output_GetDeviceName - { - public: - Result::Enum Result; - String DeviceName; - DeviceType::Enum DeviceType; - }; - - class Output_GetDeviceOutputCount - { - public: - Result::Enum Result; - unsigned int DeviceOutputCount; - }; - - class Output_GetDeviceOutputName - { - public: - Result::Enum Result; - String DeviceOutputName; - Unit::Enum DeviceOutputUnit; - }; - - class Output_GetDeviceOutputValue - { - public: - Result::Enum Result; - double Value; - bool Occluded; - }; - - class Output_GetDeviceOutputSubsamples - { - public: - Result::Enum Result; - unsigned int DeviceOutputSubsamples; - bool Occluded; - }; - - class Output_GetForcePlateCount - { - public: - Result::Enum Result; - unsigned int ForcePlateCount; - }; - - class Output_GetGlobalForceVector - { - public: - Result::Enum Result; - double ForceVector[ 3 ]; - }; - - class Output_GetGlobalMomentVector - { - public: - Result::Enum Result; - double MomentVector[ 3 ]; - }; - - class Output_GetGlobalCentreOfPressure - { - public: - Result::Enum Result; - double CentreOfPressure[ 3 ]; - }; - - class Output_GetForcePlateSubsamples - { - public: - Result::Enum Result; - unsigned int ForcePlateSubsamples; - }; - - class Output_GetEyeTrackerCount - { - public: - Result::Enum Result; - unsigned int EyeTrackerCount; - }; - - class Output_GetEyeTrackerGlobalPosition - { - public: - Result::Enum Result; - double Position[ 3 ]; - bool Occluded; - }; - - class Output_GetEyeTrackerGlobalGazeVector - { - public: - Result::Enum Result; - double GazeVector[ 3 ]; - bool Occluded; - }; - - class Output_GetCameraCount - { - public: - Result::Enum Result; - unsigned int CameraCount; - }; - - class Output_GetCameraName - { - public: - Result::Enum Result; - String CameraName; - }; - - class Output_GetCameraId - { - public: - Result::Enum Result; - unsigned int CameraId; - }; - - class Output_GetCameraUserId - { - public: - Result::Enum Result; - unsigned int CameraUserId; - }; - - class Output_GetCameraType - { - public: - Result::Enum Result; - String CameraType; - }; - - class Output_GetCameraDisplayName - { - public: - Result::Enum Result; - String CameraDisplayName; - }; - - class Output_GetCameraResolution - { - public: - Result::Enum Result; - unsigned int ResolutionX; - unsigned int ResolutionY; - }; - - class Output_GetIsVideoCamera - { - public: - Result::Enum Result; - bool IsVideoCamera; - }; - - class Output_GetCentroidCount - { - public: - Result::Enum Result; - unsigned int CentroidCount; - }; - - class Output_GetCentroidPosition - { - public: - Result::Enum Result; - double CentroidPosition[ 2 ]; - double Radius; -// double Accuracy; - }; - - class Output_GetCentroidWeight - { - public: - Result::Enum Result; - double Weight; - }; - - class Output_GetGreyscaleBlobCount - { - public: - Result::Enum Result; - unsigned int BlobCount; - }; - - class Output_GetGreyscaleBlob - { - public: - Result::Enum Result; - std::vector< unsigned int > BlobLinePositionsX; - std::vector< unsigned int > BlobLinePositionsY; - std::vector< std::vector< unsigned char > > BlobLinePixelValues; - }; - - /// Vicon DataStream SDK client. - /// The core client class for C++. - class CLASS_DECLSPEC IDataStreamClientBase - { - public: - - /// Get the version of the Vicon DataStream SDK. - /// + **Major** - /// When this number increases, we break backward compatibility with previous major versions. - /// + **Minor** - /// When this number increases, we have probably added new functionality to the SDK without breaking - /// backward compatibility with previous versions. - /// + **Point** - /// When this number increases, we have introduced a bug fix or performance enhancement without breaking - /// backward compatibility with previous versions. - /// - /// The function can be called without the client being connected. - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// Output_GetVersion Output = MyClient.GetVersion(); - /// - /// \return Output_GetVersion class containing the version information. - virtual Output_GetVersion GetVersion() const = 0; - - /// Disconnect from the Vicon DataStream Server. - /// - /// See Also: Connect(), IsConnected() - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_Disconnect Output = MyClient.Disconnect(); - /// - /// \return An Output_Disconnect class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + NotConnected - virtual Output_Disconnect Disconnect() = 0; - - /// Discover whether client is connected to the Vicon DataStream Server. - /// - /// See Also: Connect(), Disconnect() - /// - /// ViconDataStreamSDK::CPP::CPP::Client MyClient; - /// Output_IsConnected Output = MyClient.IsConnected() - /// // Output.Connected == false - /// MyClient.Connect( "localhost" ); - /// Output_IsConnected Output = MyClient.IsConnected() - /// // Output.Connected == true - /// // (assuming localhost is serving) - /// - /// \return An Output_IsConnected class containing a true value for Connected if you are - /// connected to the stream, otherwise false. - virtual Output_IsConnected IsConnected() const = 0; - - - /// Remaps the 3D axis. - /// Vicon Data uses a right-handed coordinate system, with +X forward, +Y left, and +Z up. - /// Other systems use different coordinate systems. The SDK can transform its data into any valid right-handed coordinate system by re-mapping each axis. - /// Valid directions are "Up", "Down", "Left", "Right", "Forward", and "Backward". Note that "Forward" means moving away from you, and "Backward" is moving towards you. - /// Common usages are - /// Z-up: SetAxisMapping( Forward, Left, Up ) - /// Y-up: SetAxisMapping( Forward, Up, Right ) - /// - /// See Also: GetAxisMapping() - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.SetAxisMapping( ViconDataStreamSDK::CPP::Direction::Forward, - /// ViconDataStreamSDK::CPP::Direction::Left, - /// ViconDataStreamSDK::CPP::Direction::Up ); - /// - /// MyClient = Client(); - /// MyClient.SetAxisMapping( Direction.Forward, Direction.Left, Direction.Up ); - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.SetAxisMapping( ViconDataStreamSDK.DotNET.Direction.Forward, - /// ViconDataStreamSDK.DotNET.Direction.Left, - /// ViconDataStreamSDK.DotNET.Direction.Up ); - /// - /// \param XAxis Specify the direction of your X axis relative to yourself as the observer. - /// \param YAxis Specify the direction of your Y axis relative to yourself as the observer. - /// \param ZAxis Specify the direction of your Z axis relative to yourself as the observer. - /// - /// \return An Output_SetAxisMapping class containing the result of the operation. - /// - The Result will be: - /// + Success - /// + CoLinearAxes - /// + LeftHandedAxes - virtual Output_SetAxisMapping SetAxisMapping( const Direction::Enum XAxis, const Direction::Enum YAxis, const Direction::Enum ZAxis ) = 0; - - /// Get the current Axis mapping. - /// - /// See Also: SetAxisMapping() - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// Output_GetAxisMapping Output = MyClient.GetAxisMapping(); - /// // Output.XAxis == ViconDataStreamSDK::CPP::Direction::Forward - /// // Output.YAxis == ViconDataStreamSDK::CPP::Direction::Left - /// // Output.ZAxis == ViconDataStreamSDK::CPP::Direction::Up - /// - /// MyClient = Client(); - /// Output = MyClient.GetAxisMapping(); - /// % Output.XAxis == Direction.Forward - /// % Output.YAxis == Direction.Left - /// % Output.ZAxis == Direction.Up - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// Output_GetAxisMapping Output = MyClient.GetAxisMapping(); - /// // Output.XAxis == ViconDataStreamSDK.DotNET.Direction.Forward - /// // Output.YAxis == ViconDataStreamSDK.DotNET.Direction.Left - /// // Output.ZAxis == ViconDataStreamSDK.DotNET.Direction.Up - /// - /// \return An Output_GetAxisMapping class containing the result of the operation. - /// - The Result will be: - /// + XAxis, YAxis, ZAxis - virtual Output_GetAxisMapping GetAxisMapping() const = 0; - - /// Return the number of subjects in the DataStream. This information can be used in conjunction with GetSubjectName. - /// - /// See Also: GetSubjectName() - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// Output_GetSubjectCount Output; - /// Output = MyClient.GetSubjectCount(); // Output.Result == NoFrame - /// // Ooutput.SubjectCount == 0 - /// MyClient.GetFrame(); - /// Output = MyClient.GetSubjectCount(); // Output.Result == Success - /// // Output.SubjectCount >= 0 - /// - /// MyClient = Client(); - /// MyClient.Connect( 'localhost' ); - /// Output = MyClient.GetSubjectCount(); % Output.Result == NoFrame - /// % Ooutput.SubjectCount == 0 - /// MyClient.GetFrame(); - /// Output = MyClient.GetSubjectCount(); % Output.Result == Success - /// % Output.SubjectCount >= 0 - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// Output_GetSubjectCount Output; - /// Output = MyClient.GetSubjectCount(); // Output.Result == NoFrame - /// // Ooutput.SubjectCount == 0 - /// MyClient.GetFrame(); - /// Output = MyClient.GetSubjectCount(); // Output.Result == Success - /// // Output.SubjectCount >= 0 - /// - /// \return An Output_GetSubjectCount class containing the result of the operation and the number of subjects. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - virtual Output_GetSubjectCount GetSubjectCount() const = 0; - - /// Return the name of a subject. This can be passed into segment and marker functions. - /// - /// See Also: GetSubjectCount() - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSubjectCount OutputGSC; - /// OutputGSC = MyClient.GetSubjectCount(); // OutputGSC.Result == Success - /// // OutputGSC.SubjectCount == 2 - /// Output_GetSubjectName OutputGSN; - /// OutputGSN = MyClient.GetSubjectName(0);// OutputGSN.Result == Success - /// // OutputGSN.SubjectName == "Al" - /// OutputGSN = MyClient.GetSubjectName(1);// OutputGSN.Result == Success - /// // OutputGSN .SubjectName == "Bob" - /// OutputGSN = MyClient.GetSubjectName(2);// OutputGSN.Result == InvalidIndex - /// // OutputGSN.SubjectName == "" - /// - /// MyClient = Client; - /// MyClient.Connect( 'localhost' ); - /// MyClient.GetFrame(); - /// OutputGSC = MyClient.GetSubjectCount(); % OutputGSC.Result == Success - /// % OutputGSC.SubjectCount == 2 - /// OutputGSN = MyClient.GetSubjectName(1); % OutputGSN.Result == Success - /// % OutputGSN.SubjectName == 'Al' - /// OutputGSN = MyClient.GetSubjectName(2); % OutputGSN.Result == Success - /// % OutputGSN .SubjectName == 'Bob' - /// OutputGSN = MyClient.GetSubjectName(3); % OutputGSN.Result == InvalidIndex - /// % OutputGSN.SubjectName == '' - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = - /// new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSubjectCount OutputGSC; - /// OutputGSC = MyClient.GetSubjectCount(); // OutputGSC.Result == Success - /// // OutputGSC.SubjectCount == 2 - /// Output_GetSubjectName OutputGSN; - /// OutputGSN = MyClient.GetSubjectName(0);// OutputGSN.Result == Success - /// // OutputGSN.SubjectName == "Al" - /// OutputGSN = MyClient.GetSubjectName(1);// OutputGSN.Result == Success - /// // OutputGSN .SubjectName == "Bob" - /// OutputGSN = MyClient.GetSubjectName(2);// OutputGSN.Result == InvalidIndex - /// // OutputGSN.SubjectName == "" - /// - /// - /// \param Subject Index The index of the subject. A valid Subject Index is between 0 and GetSubjectCount()-1. - // Matlab: A valid Subject Index is between 1 and GetSubjectCount(). - /// \return An Output_GetSubjectName GetSubjectName class containing the result of the operation and the name of the subject. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - virtual Output_GetSubjectName GetSubjectName( const unsigned int SubjectIndex ) const = 0; - - /// Return the name of the root segment for a specified subject. This can be passed into segment functions.The root segment is the ancestor of all other segments in the subject. - /// - /// See Also: GetSegmentCount(), GetSegmentParentName(), GetSegmentChildCount(), GetSegmentChildName() - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSubjectRootSegmentName Output; - /// Output = MyClient.GetSubjectRootSegmentName( "Bob" ); - /// // Output.Result == Success - /// // Output.SegmentName == "Pelvis" - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSubjectRootSegmentName( "Bob" ); - /// % Output.Result == Success - /// % Output.SegmentName == "Pelvis" - /// - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSubjectRootSegmentName Output; - /// Output = MyClient.GetSubjectRootSegmentName( "Bob" ); - /// // Output.Result == Success - /// // Output.SegmentName == "Pelvis" - /// - /// \param Subject Name The name of the subject - /// \return An Output_GetSubjectRootSegmentName class containing the result of the operation and the name of the root segment. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - virtual Output_GetSubjectRootSegmentName GetSubjectRootSegmentName( const String & SubjectName ) const = 0; - - /// Return the number of segments for a specified subject in the DataStream. This information can be used in conjunction with GetSegmentName. - /// - /// See Also: GetSubjectName(), GetSegmentName() - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.EnableSegmentData(); - /// MyClient.Connect( "localhost" ); - /// Output_GetSegmentCount Output; - /// Output = MyClient.GetSegmentCount( "Bob" ); // Output.Result == NoFrame - /// // Output.SegmentCount == 0 - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentCount( "Al" ); // Output.Result == - /// // InvalidSubjectName - /// // Output.SegmentCount == 0 - /// Output = MyClient.GetSegmentCount( "Bob" );// Output.Result == Success - /// // Output.SegmentCount >= 0 - /// - /// MyClient = Client(); - /// MyClient.EnableSegmentData(); - /// MyClient.Connect( "localhost" ); - /// Output = MyClient.GetSegmentCount( "Bob" ); % Output.Result == NoFrame - /// % Output.SegmentCount == 0 - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentCount( "Al" ); % Output.Result == - /// % InvalidSubjectName - /// % Output.SegmentCount == 0 - /// Output = MyClient.GetSegmentCount( "Bob" ); % Output.Result == Success - /// % Output.SegmentCount >= 0 - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.EnableSegmentData(); - /// MyClient.Connect( "localhost" ); - /// Output_GetSegmentCount Output; - /// Output = MyClient.GetSegmentCount( "Bob" ); // Output.Result == NoFrame - /// // Output.SegmentCount == 0 - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentCount( "Al" ); // Output.Result == - /// // InvalidSubjectName - /// // Output.SegmentCount == 0 - /// Output = MyClient.GetSegmentCount( "Bob" ); // Output.Result == Success - /// // Output.SegmentCount >= 0 - /// - /// \param Subject Name The name of the subject. - /// \return An Output_GetSegmentCount class containing the result of the operation and the number of segments. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - virtual Output_GetSegmentCount GetSegmentCount( const String & SubjectName ) const = 0; - - /// Return the name of a subject segment specified by index. - /// - /// See Also: GetSegmentCount(), GetSegmentChildCount(), GetSegmentChildName(), GetSubjectRootSegmentName() - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentName Output; - /// // SegmentIndex must be between 0 and GetSegmentCount() - 1 - /// Output = MyClient.GetSegmentName( "Bob", 0 ); - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// % SegmentIndex must be between 1 and GetSegmentCount() - /// Output = MyClient.GetSegmentName( "Bob", 1 ); - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentParentName Output; - /// // SegmentIndex must be between 0 and GetSegmentCount() - 1 - /// Output = MyClient.GetSegmentName( "Bob", 0 ); - /// - /// \param Subject Name The name of the subject - /// \param Segment Index The index of the segment - /// \return An Output_GetSegmentName class containing the result of the operation and the name of the parent segment or an empty string if it is the root segment. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - /// + InvalidSubjectName - virtual Output_GetSegmentName GetSegmentName( const String & SubjectName, const unsigned int SegmentIndex ) const = 0; - /// Return the name of a child segment for a specified subject segment. This can be passed into segment functions. - /// - /// See Also: GetSegmentCount() - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentChildCount OutputGSCC; - /// OutputGSCC = MyClient.GetSegmentChildCount( "Bob", "Pelvis" ); - /// // OutputGSCC.Result == Success - /// // OutputGSCC.SegmentCount == 2 - /// Output_GetSegmentChildName OutputGSCN; - /// OutputGSCN = MyClient.GetSegmentName( "Alice", 0 ); - /// // OutputGSN.Result == InvalidSubjectName - /// // OutputGSN.SegmentName == "" - /// OutputGSCN = MyClient.GetSegmentName( "Bob", "Pelvis", 0 ); - /// // OutputGSCN.Result == Success - /// // OutputGSCN.SegmentName == "LFemur" - /// OutputGSCN = MyClient.GetSegmentName( "Bob", "Pelvis", 1 ); - /// // OutputGSCN.Result == Success - /// // OutputGSCN.SegmentName == "RFemur" - /// OutputGSCN = MyClient.GetSegmentName( "Bob", "Pelvis", 2 ); - /// // OutputGSCN.Result == InvalidIndex - /// // OutputGSCN.SegmentName == "" - /// // (no third segment) - /// - /// A valid Segment Index is between 1 and GetSegmentChildCount() - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// OutputGSCC = MyClient.GetSegmentChildCount( "Bob", "Pelvis" ); - /// % OutputGSCC.Result == Success - /// % OutputGSCC.SegmentCount == 2 - /// OutputGSCN = MyClient.GetSegmentChildName( "Alice", "Pelvis", 1 ); - /// % OutputGSCN.Result == InvalidSubjectName - /// % OutputGSCN.SegmentName == "" - /// OutputGSCN = MyClient.GetSegmentChildName( "Bob", "Pelvis", 1 ); - /// % OutputGSCN.Result == Success - /// % OutputGSCN.SegmentName == "LFemur" - /// OutputGSCN = MyClient.GetSegmentChildName( "Bob", "Pelvis", 2 ); - /// % OutputGSCN.Result == Success - /// % OutputGSCN.SegmentName == "RFemur" - /// OutputGSCN = MyClient.GetSegmentChildName( "Bob", "Pelvis", 3 ); - /// % OutputGSCN.Result == InvalidIndex - /// % OutputGSCN.SegmentName == "" - /// % (no third segment) - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentChildCount OutputGSCC; - /// OutputGSCC = MyClient.GetSegmentChildCount( "Bob", "Pelvis" ); - /// // OutputGSCC.Result == Success - /// // OutputGSCC.SegmentCount == 2 - /// Output_GetSegmentChildName OutputGSCN; - /// OutputGSCN = MyClient.GetSegmentChildName( "Alice", "Pelvis", 0 ); - /// // OutputGSCN.Result == InvalidSubjectName - /// // OutputGSCN.SegmentName == "" - /// OutputGSCN = MyClient.GetSegmentChildName( "Bob", "Pelvis", 0 ); - /// // OutputGSCN.Result == Success - /// // OutputGSCN.SegmentName == "LFemur" - /// OutputGSCN = MyClient.GetSegmentChildName( "Bob", "Pelvis", 1 ); - /// // OutputGSCN.Result == Success - /// // OutputGSCN.SegmentName == "RFemur" - /// OutputGSCN = MyClient.GetSegmentChildName( "Bob", "Pelvis", 2 ); - /// // OutputGSCN.Result == InvalidIndex - /// // OutputGSCN.SegmentName == "" - /// // (no third segment) - /// - /// \param Subject Name The name of the subject - /// \param Segment Name The name of the segment - /// \param Segment Index The index of the segment. A valid Segment Index is between 0 and GetSegmentChildCount()-1. - /// \return An Output_GetSegmentChildCount class containing the result of the operation and the name of the child segment. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - /// + InvalidSubjectName - /// + InvalidSegmentName - virtual Output_GetSegmentChildCount GetSegmentChildCount( const String & SubjectName, const String & SegmentName ) const = 0; - - /// Return the name of the child segment for a specified subject segment and index. - /// - /// See Also: GetSegmentCount(), GetSegmentChildCount(), GetSegmentChildName(), GetSubjectRootSegmentName() - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentChildName Output; - /// // Segment index must be between 0 and GetSegmentChildCount() - 1 - /// Output = MyClient.GetSegmentChildName( "Bob", "Pelvis", 0 ); - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// // Segment index must be between 1 and GetSegmentChildCount() - /// Output = MyClient.GetSegmentChildName( "Bob", "Pelvis", 1 ); - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentChildName Output; - /// // Segment index must be between 0 and GetSegmentChildCount() - 1 - /// Output = MyClient.GetSegmentChildName( "Bob", "Pelvis", 0 ); - /// - /// - /// \param Subject Name The name of the subject - /// \param Segment Name The name of the segment - /// \param Segment Index The index of the child segment. A valid Segment Index is between 0 and GetSegmentChildCount()-1. - /// \return An Output_GetSegmentChildName class containing the result of the operation and the name of the child segment. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidIndex - /// + InvalidSubjectName - /// + InvalidSegmentName - virtual Output_GetSegmentChildName GetSegmentChildName( const String & SubjectName, const String & SegmentName, const unsigned int SegmentIndex ) const = 0; - - /// Return the name of the parent segment for a specified subject segment. If the specified segment is the root segment of the subject then it will return an empty string. - /// - /// See Also: GetSegmentCount(), GetSegmentChildCount(), GetSegmentChildName(), GetSubjectRootSegmentName() - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentParentName Output; - /// Output = MyClient.GetSegmentParentName( "Bob", "Pelvis" ); - /// // Output.Result == Success - /// // Output.SegmentName == "" - /// // This is the root segment - /// Output = MyClient.GetSegmentParentName( "Bob", "LFemur" ); - /// // Output.Result == Success - /// // Output.SegmentName == "Pelvis" - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentParentName( "Bob", "Pelvis" ); - /// % Output.Result == Success - /// % Output.SegmentCount == "" - /// % This is the root segment - /// Output = MyClient.GetSegmentParentName( "Bob", "LFemur" ); - /// % Output.Result == Success - /// % Output.SegmentCount == "Pelvis" - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentParentName Output; - /// Output = MyClient.GetSegmentParentName( "Bob", "Pelvis" ); - /// // Output.Result == Success - /// // Output.SegmentName == "" - /// // This is the root segment - /// Output = MyClient.GetSegmentParentName( "Bob", "LFemur" ); - /// // Output.Result == Success - /// // Output.SegmentName == "Pelvis" - /// - /// \param Subject Name The name of the subject - /// \param Segment Name The name of the segment - /// \return An Output_GetSegmentParentName class containing the result of the operation and the name of the parent segment or an empty string if it is the root segment. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - virtual Output_GetSegmentParentName GetSegmentParentName( const String & SubjectName, const String & SegmentName ) const = 0; - /// Return the static pose translation of a subject segment. - /// - /// See Also: GetSegmentStaticRotationHelical(), GetSegmentStaticRotationMatrix(), GetSegmentStaticRotationQuaternion(), GetSegmentStaticRotationEulerXYZ(), GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ() - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentStaticTranslation Output = - /// MyClient.GetSegmentStaticTranslation( "Alice", "Pelvis" ); - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentStaticTranslation( "Alice", "Pelvis" ); - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentStaticTranslation Output = - /// MyClient.GetSegmentStaticTranslations( "Alice", "Pelvis" ); - /// - /// \param Subject Name The name of the subject - /// \param Segment Name The name of the segment - /// \return An Output_GetSegmentStaticTranslation class containing the result of the operation and the translation of the segment. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - virtual Output_GetSegmentStaticTranslation GetSegmentStaticTranslation( const String & SubjectName, const String & SegmentName ) const = 0; - /// Return the static pose rotation of a subject segment in helical coordinates. - /// The helical coordinates represent a vector whose length is the amount of rotation in radians, and the direction is the axis about which to rotate. - /// - /// See Also: GetSegmentStaticTranslation(), GetSegmentStaticRotationMatrix(), GetSegmentStaticRotationQuaternion(), GetSegmentStaticRotationEulerXYZ(), GetSegmentLocalTranslation, GetSegmentLocalRotationHelical, GetSegmentLocalRotationMatrix, GetSegmentLocalRotationQuaternion, GetSegmentLocalRotationEulerXYZ - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentStaticRotationHelical Output = - /// MyClient.GetSegmentStaticRotationHelical( "Alice", "Pelvis" ); - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentStaticRotationHelical( "Alice", "Pelvis" ); - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentStaticRotationHelical Output = - /// MyClient.GetSegmentStaticRotationHelical( "Alice", "Pelvis" ); - /// - /// \param Subject Name The name of the subject - /// \param Segment Name The name of the segment - /// \return An Output_GetSegmentStaticRotationHelical class containing the result of the operation and the rotation of the segment. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - virtual Output_GetSegmentStaticRotationHelical GetSegmentStaticRotationHelical( const String & SubjectName, const String & SegmentName ) const = 0; - /// Return the static pose rotation of a subject segment as a 3x3 row-major matrix. - /// - /// See Also: GetSegmentStaticTranslation(), GetSegmentStaticRotationHelical(), GetSegmentStaticRotationQuaternion(), GetSegmentStaticRotationEulerXYZ(), GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ() - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentStaticRotationMatrix Output = - /// MyClient.GetSegmentStaticRotationMatrix( "Alice", "Pelvis" ); - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentStaticRotationMatrix( "Alice", "Pelvis" ); - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentStaticRotationMatrix Output = - /// MyClient.GetSegmentStaticRotationMatrix( "Alice", "Pelvis" ); - /// - /// \param Subject Name The name of the subject - /// \param Segment Name The name of the segment - /// \return An Output_GetSegmentStaticRotationMatrix class containing the result of the operation and the rotation of the segment. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - virtual Output_GetSegmentStaticRotationMatrix GetSegmentStaticRotationMatrix( const String & SubjectName, const String & SegmentName ) const = 0; - /// Return the static pose rotation of a subject segment in quaternion coordinates. - /// The quaternion is of the form (x, y, z, w) where w is the real component and x, y and z are the imaginary components. - /// N.B. This is different from that used in many other applications, which use (w, x, y, z). - /// - /// See Also: GetSegmentStaticTranslation(), GetSegmentStaticRotationHelical(), GetSegmentStaticRotationMatrix(), GetSegmentStaticRotationEulerXYZ(), GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ() - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentStaticRotationQuaternion Output = - /// MyClient.GetSegmentStaticRotationQuaternion( "Alice", "Pelvis" ); - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentStaticRotationQuaternion( "Alice", "Pelvis" ); - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentStaticRotationQuaternion Output = - /// MyClient.GetSegmentStaticRotationQuaternion( "Alice", "Pelvis" ); - /// - /// - /// \param Subject Name The name of the subject - /// \param Segment Name The name of the segment - /// \return An Output_GetSegmentStaticRotationQuaternion class containing the result of the operation and the rotation of the segment. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - virtual Output_GetSegmentStaticRotationQuaternion GetSegmentStaticRotationQuaternion( const String & SubjectName, const String & SegmentName ) const = 0; - - /// Return the static pose rotation of a subject segment in Euler XYZ coordinates. - /// - /// See Also: GetSegmentStaticTranslation(), GetSegmentStaticRotationHelical(), GetSegmentStaticRotationMatrix(), - /// GetSegmentStaticRotationQuaternion(), GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), - /// GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ(). - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentStaticRotationEulerXYZ Output; - /// Output = MyClient.GetSegmentStaticRotationEulerXYZ( "Alice", "Pelvis" ); - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentStaticRotationEulerXYZ( "Alice", "Pelvis" ); - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentStaticRotationEulerXYZ Output = - /// MyClient.GetSegmentStaticRotationEulerXYZ( "Alice", "Pelvis" ); - /// - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentStaticRotationEulerXYZ class containing the result of the request - /// and the rotation of the segment \f$(x,y,z)\f$. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - virtual Output_GetSegmentStaticRotationEulerXYZ GetSegmentStaticRotationEulerXYZ( const String & SubjectName, const String & SegmentName ) const = 0; - /// Return the translation of a subject segment in global coordinates. - /// The translation is of the form ( x, y, z ) where x, y and z are in millimeters with respect to the global origin. - /// - /// See Also: GetSegmentGlobalRotationHelical(), GetSegmentGlobalRotationMatrix(), GetSegmentGlobalRotationQuaternion(), GetSegmentGlobalRotationEulerXYZ(), GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ() - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentGlobalTranslation Output = - /// MyClient.GetSegmentGlobalTranslation( "Alice", "Pelvis" ); - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentGlobalTranslation( "Alice", "Pelvis" ); - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = - /// new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentGlobalTranslation Output = - /// MyClient.GetSegmentGlobalTranslations( "Alice", "Pelvis" ); - /// - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentGlobalTranslation class containing the result of the operation, the translation of the segment, and whether the segment is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - /// - Occluded will be True if the segment was absent at this frame. In this case the translation will be [0,0,0]. - virtual Output_GetSegmentGlobalTranslation GetSegmentGlobalTranslation( const String & SubjectName, const String & SegmentName ) const = 0; - /// Return the rotation of a subject segment in global helical coordinates. - /// - /// See Also: GetSegmentGlobalTranslation(), GetSegmentGlobalRotationMatrix(), GetSegmentGlobalRotationQuaternion(), GetSegmentGlobalRotationEulerXYZ(), GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ() - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentGlobalRotationHelical Output = - /// MyClient.GetSegmentGlobalRotationHelical( "Alice", "Pelvis" ); - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentGlobalRotationHelical( "Alice", "Pelvis" ); - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentGlobalRotationHelical Output = - /// MyClient.GetSegmentGlobalRotationHelical( "Alice", "Pelvis" ); - /// - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentGlobalRotationHelical class containing the result of the operation, the rotation of the segment, and whether the segment is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - /// - Occluded will be True if the segment was absent at this frame. In this case, the rotation will be [0,0,0]. - virtual Output_GetSegmentGlobalRotationHelical GetSegmentGlobalRotationHelical( const String & SubjectName, const String & SegmentName ) const = 0; - - /// Return the rotation of a subject segment as a 3x3 row-major matrix in global coordinates. - /// - /// See Also: GetSegmentGlobalTranslation(), GetSegmentGlobalRotationHelical(), GetSegmentGlobalRotationQuaternion(), GetSegmentGlobalRotationEulerXYZ(), GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ() - /// - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentGlobalRotationMatrix Output = - /// MyClient.GetSegmentGlobalRotationMatrix( "Alice", "Pelvis" ); - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentGlobalRotationMatrix( "Alice", "Pelvis" ); - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentGlobalRotationMatrix Output = - /// MyClient.GetSegmentGlobalRotationMatrix( "Alice", "Pelvis" ); - /// - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentGlobalRotationMatrix Class containing the result of the operation, the rotation of the segment, and whether the segment is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - /// - Occluded will be True if the segment was absent at this frame. - virtual Output_GetSegmentGlobalRotationMatrix GetSegmentGlobalRotationMatrix( const String & SubjectName, const String & SegmentName ) const = 0; - /// Return the rotation of a subject segment in global quaternion coordinates. - /// The quaternion is of the form (x, y, z, w) where w is the real component and x, y and z are the imaginary components. - /// N.B. This is different from that used in many other applications, which use (w, x, y, z). - /// - /// See Also: GetSegmentGlobalTranslation(), GetSegmentGlobalRotationHelical(), GetSegmentGlobalRotationMatrix(), GetSegmentGlobalRotationEulerXYZ(), GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ() - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentGlobalRotationQuaternion Output = - /// MyClient.GetSegmentGlobalRotationQuaternion( "Alice", "Pelvis" ); - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentGlobalRotationQuaternion( "Alice", "Pelvis" ); - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentGlobalRotationQuaternion Output = - /// MyClient.GetSegmentGlobalRotationQuaternion( "Alice", "Pelvis" ); - /// - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentGlobalRotationQuaternion class containing the result of the operation, the rotation of the segment, and whether the segment is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - /// - Occluded will be True if the segment was absent at this frame. In this case the Rotation will be [1,0,0,0]. - virtual Output_GetSegmentGlobalRotationQuaternion GetSegmentGlobalRotationQuaternion( const String & SubjectName, const String & SegmentName ) const = 0; - /// Return the rotation of a subject segment in global Euler XYZ coordinates. - /// - /// See Also: GetSegmentGlobalTranslation(), GetSegmentGlobalRotationHelical(), GetSegmentGlobalRotationMatrix(), GetSegmentGlobalRotationQuaternion(), GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ() - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentGlobalRotationEulerXYZ Output = - /// MyClient.GetSegmentGlobalRotationEulerXYZ( "Alice", "Pelvis" ); - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentGlobalRotationEulerXYZ( "Alice", "Pelvis" ); - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentGlobalRotationEulerXYZ Output = - /// MyClient.GetSegmentGlobalRotationEulerXYZ( "Alice", "Pelvis" ); - /// - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentGlobalRotationEulerXYZ class containing the result of the operation, the rotation of the segment, and whether the segment is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - /// - Occluded will be True if the segment was absent at this frame. In this case the rotation will be [0,0,0]. - virtual Output_GetSegmentGlobalRotationEulerXYZ GetSegmentGlobalRotationEulerXYZ( const String & SubjectName, const String & SegmentName ) const = 0; - - /// Return the translation of a subject segment in local coordinates relative to its parent segment. - /// - /// See Also: GetSegmentLocalRotationHelical(), GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ(), GetSegmentGlobalTranslation(), GetSegmentGlobalRotationHelical(), GetSegmentGlobalRotationMatrix(), GetSegmentGlobalRotationQuaternion(), GetSegmentGlobalRotationEulerXYZ() - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentLocalTranslation Output = - /// MyClient.GetSegmentLocalTranslation( "Alice", "Pelvis" ); - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentLocalTranslation( "Alice", "Pelvis" ); - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.EnableSegmentData(); - /// MyClient.GetFrame(); - /// Output_GetSegmentLocalTranslation Output = - /// MyClient.GetSegmentLocalTranslations( "Alice", "Pelvis" ); - /// - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentLocalTranslation class containing the result of the operation, the translation of the segment, and whether the segment is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - /// - Occluded will be True if the segment was absent at this frame. In this case the translation will be [0,0,0]. - virtual Output_GetSegmentLocalTranslation GetSegmentLocalTranslation( const String & SubjectName, const String & SegmentName ) const = 0; - - /// Return the rotation of a subject segment in local helical coordinates relative to its parent segment. - /// - /// See Also: GetSegmentLocalTranslation(), GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ(), GetSegmentGlobalTranslation(), GetSegmentGlobalRotationHelical(), GetSegmentGlobalRotationMatrix(), GetSegmentGlobalRotationQuaternion(), GetSegmentGlobalRotationEulerXYZ() - /// - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentLocalRotationHelical Output = - /// MyClient.GetSegmentLocalRotationHelical( "Alice", "Pelvis" ); - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentLocalRotationHelical( "Alice", "Pelvis" ); - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentLocalRotationHelical Output = - /// MyClient.GetSegmentLocalRotationHelical( "Alice", "Pelvis" ); - /// - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentLocalRotationHelical class containing the result of the operation, the rotation of the segment, and whether the segment is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - /// - Occluded will be True if the segment was absent at this frame. In this case the Rotation will be [0,0,0]. - virtual Output_GetSegmentLocalRotationHelical GetSegmentLocalRotationHelical( const String & SubjectName, const String & SegmentName ) const = 0; - - /// Return the rotation row-major matrix of a subject segment in local coordinates relative to its parent segment. - /// - /// See Also: GetSegmentLocalTranslation(), GetSegmentLocalRotationQuaternion(), GetSegmentLocalRotationEulerXYZ(), GetSegmentGlobalTranslation(), GetSegmentGlobalRotationHelical(), GetSegmentGlobalRotationMatrix() , GetSegmentGlobalRotationQuaternion(), GetSegmentGlobalRotationEulerXYZ() - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentLocalRotationMatrix Output = - /// MyClient.GetSegmentLocalRotationMatrix( "Alice", "Pelvis" ); - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentLocalRotationMatrix( "Alice", "Pelvis" ); - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentLocalRotationMatrix Output = - /// MyClient.GetSegmentLocalRotationMatrix( "Alice", "Pelvis" ); - /// - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentLocalRotationMatrix class containing the result of the operation, the rotation of the segment, and whether the segment is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - /// - Occluded will be True if the segment was absent at this frame. - // - virtual Output_GetSegmentLocalRotationMatrix GetSegmentLocalRotationMatrix( const String & SubjectName, const String & SegmentName ) const = 0; - /// Return the rotation of a subject segment in local quaternion coordinates relative to its parent segment. - /// The quaternion is of the form (x, y, z, w) where w is the real component and x, y and z are the imaginary components. N.B. This is different from that used in many other applications, which use (w, x, y, z). - /// - /// See Also: GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationEulerXYZ(), GetSegmentGlobalTranslation(), GetSegmentGlobalRotationHelical(), GetSegmentGlobalRotationMatrix(), GetSegmentGlobalRotationQuaternion(), GetSegmentGlobalRotationEulerXYZ() - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentLocalRotationQuaternion Output = - /// MyClient.GetSegmentLocalRotationQuaternion( "Alice", "Pelvis" ); - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentLocalRotationQuaternion( "Alice", "Pelvis" ); - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentLocalRotationQuaternion Output = - /// MyClient.GetSegmentLocalRotationQuaternion( "Alice", "Pelvis" ); - /// - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentLocalRotationQuaternion class containing the result of the operation, the rotation of the segment, and whether the segment is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - /// - Occluded will be True if the segment was absent at this frame. In this case the rotation will be [1,0,0,0]. - virtual Output_GetSegmentLocalRotationQuaternion GetSegmentLocalRotationQuaternion( const String & SubjectName, const String & SegmentName ) const = 0; - - /// Return the rotation of a subject segment in local Euler XYZ coordinates relative to its parent segment. - /// - /// See Also: GetSegmentLocalTranslation(), GetSegmentLocalRotationHelical(), GetSegmentLocalRotationMatrix(), GetSegmentLocalRotationQuaternion(), GetSegmentGlobalTranslation(), GetSegmentGlobalRotationHelical(), GetSegmentGlobalRotationMatrix() , GetSegmentGlobalRotationQuaternion(), GetSegmentGlobalRotationEulerXYZ() - /// - /// - /// ViconDataStreamSDK::CPP::Client MyClient; - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentLocalRotationEulerXYZ Output = - /// MyClient.GetSegmentLocalRotationEulerXYZ( "Alice", "Pelvis" ); - /// - /// MyClient = Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output = MyClient.GetSegmentLocalRotationEulerXYZ( "Alice", "Pelvis" ); - /// - /// ViconDataStreamSDK.DotNET.Client MyClient = new ViconDataStreamSDK.DotNET.Client(); - /// MyClient.Connect( "localhost" ); - /// MyClient.GetFrame(); - /// Output_GetSegmentLocalRotationEulerXYZ Output = - /// MyClient.GetSegmentLocalRotationEulerXYZ( "Alice", "Pelvis" ); - /// - /// \param SubjectName The name of the subject. - /// \param SegmentName The name of the segment. - /// \return An Output_GetSegmentLocalRotationEulerXYZ class containing the result of the operation, the rotation of the segment, and whether the segment is occluded. - /// - The Result will be: - /// + Success - /// + NotConnected - /// + NoFrame - /// + InvalidSubjectName - /// + InvalidSegmentName - /// - Occluded will be True if the segment was absent at this frame. In this case the rotation will be [0,0,0]. - virtual Output_GetSegmentLocalRotationEulerXYZ GetSegmentLocalRotationEulerXYZ( const String & SubjectName, const String & SegmentName ) const = 0; - - }; -} // End of namespace CPP -} // End of namespace ViconDataStreamSDK diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/ReadMe.txt b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/ReadMe.txt deleted file mode 100644 index 9bfb01709..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/ReadMe.txt +++ /dev/null @@ -1 +0,0 @@ -If you modify Client.h, please modify the copy in ViconDataStreamSDK_CPPPlugins \ No newline at end of file diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/StringFactory.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/StringFactory.h deleted file mode 100644 index 7cb9adab6..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/StringFactory.h +++ /dev/null @@ -1,48 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -namespace ViconDataStreamSDK -{ - namespace CPP - { - class VStringFactory : public IStringFactory - { - public: - - virtual char * AllocAndCopyString( const char * i_pSource ) - { - char * pString = new char[ strlen( i_pSource ) + 1 ]; - strcpy( pString, i_pSource ); - return pString; - } - - virtual void FreeString( char * i_pString ) - { - delete[] i_pString; - } - }; - } -} diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/VersionInfoResource.rc2 b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/VersionInfoResource.rc2 deleted file mode 100644 index d256757fa..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/VersionInfoResource.rc2 +++ /dev/null @@ -1,41 +0,0 @@ -#include "ViconDataStreamSDK_CPPVersion.h" - -///////////////////////////////////////////////////////////////////////////// -// -// Version -// - -VS_VERSION_INFO VERSIONINFO - FILEVERSION VICONDATASTREAMSDK_CPP_VERSION_MAJOR,VICONDATASTREAMSDK_CPP_VERSION_MINOR,VICONDATASTREAMSDK_CPP_VERSION_POINT,VICONDATASTREAMSDK_CPP_VERSION_REVISION - PRODUCTVERSION VICONDATASTREAMSDK_CPP_VERSION_MAJOR,VICONDATASTREAMSDK_CPP_VERSION_MINOR,VICONDATASTREAMSDK_CPP_VERSION_POINT,VICONDATASTREAMSDK_CPP_VERSION_REVISION - FILEFLAGSMASK 0x3fL -#ifdef _DEBUG - FILEFLAGS 0x1L -#else - FILEFLAGS 0x0L -#endif - FILEOS 0x4L - FILETYPE 0x1L - FILESUBTYPE 0x0L -BEGIN - BLOCK "StringFileInfo" - BEGIN - BLOCK "040904b0" - BEGIN - VALUE "CompanyName", VICONDATASTREAMSDK_CPP_COMPANY_L - VALUE "FileDescription", "Vicon DataStream SDK for C++" - VALUE "FileVersion", VICONDATASTREAMSDK_CPP_FULL_VERSION_STRING_L - VALUE "InternalName", "Vicon DataStream for C++" - VALUE "LegalCopyright", VICONDATASTREAMSDK_CPP_COPYRIGHT_L - VALUE "LegalTrademarks", VICONDATASTREAMSDK_CPP_TRADEMARK_L - VALUE "OriginalFilename", "ViconDataStreamSDK_CPP.dll" - VALUE "ProductName", "Vicon DataStream SDK" - VALUE "ProductVersion", VICONDATASTREAMSDK_CPP_FULL_VERSION_STRING_L - END - END - BLOCK "VarFileInfo" - BEGIN - VALUE "Translation", 0x409, 1200 - END -END - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/ViconDataStreamSDK_CPP.rc b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/ViconDataStreamSDK_CPP.rc deleted file mode 100644 index 7a4bb5726..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/ViconDataStreamSDK_CPP.rc +++ /dev/null @@ -1,61 +0,0 @@ -// Microsoft Visual C++ generated resource script. -// -#include "resource.h" - -#define APSTUDIO_READONLY_SYMBOLS -///////////////////////////////////////////////////////////////////////////// -// -// Generated from the TEXTINCLUDE 2 resource. -// -#include "windows.h" -///////////////////////////////////////////////////////////////////////////// -#undef APSTUDIO_READONLY_SYMBOLS - -///////////////////////////////////////////////////////////////////////////// -// English (U.S.) resources - -#if !defined(AFX_RESOURCE_DLL) || defined(AFX_TARG_ENU) -#ifdef _WIN32 -LANGUAGE LANG_ENGLISH, SUBLANG_ENGLISH_US -#pragma code_page(1252) -#endif //_WIN32 - -#ifdef APSTUDIO_INVOKED -///////////////////////////////////////////////////////////////////////////// -// -// TEXTINCLUDE -// - -1 TEXTINCLUDE -BEGIN - "resource.h\0" -END - -2 TEXTINCLUDE -BEGIN - "#include ""windows.h""\0" -END - -3 TEXTINCLUDE -BEGIN - "#include ""VersionInfoResource.rc2""\r\n" - "\0" -END - -#endif // APSTUDIO_INVOKED - -#endif // English (U.S.) resources -///////////////////////////////////////////////////////////////////////////// - - - -#ifndef APSTUDIO_INVOKED -///////////////////////////////////////////////////////////////////////////// -// -// Generated from the TEXTINCLUDE 3 resource. -// -#include "VersionInfoResource.rc2" - -///////////////////////////////////////////////////////////////////////////// -#endif // not APSTUDIO_INVOKED - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/ViconDataStreamSDK_CPPVersion.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/ViconDataStreamSDK_CPPVersion.h deleted file mode 100644 index cd6ae3559..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/ViconDataStreamSDK_CPPVersion.h +++ /dev/null @@ -1,27 +0,0 @@ -#define VICONDATASTREAMSDK_CPP_VERSION_MAJOR 1 -#define VICONDATASTREAMSDK_CPP_VERSION_MINOR 8 -#define VICONDATASTREAMSDK_CPP_VERSION_POINT 0 -#define VICONDATASTREAMSDK_CPP_VERSION_REVISION_CONTROL_SYSTEM "unknown" -#define VICONDATASTREAMSDK_CPP_VERSION_REVISION 0 -#define VICONDATASTREAMSDK_CPP_VERSION_MONOTONIC_REVISION 0 -#define VICONDATASTREAMSDK_CPP_VERSION_PUBLIC_REVISION_STRING "0" -#define VICONDATASTREAMSDK_CPP_VERSION_RELEASE "" -#define VICONDATASTREAMSDK_CPP_VERSION_BRANCH "unknown" -#define VICONDATASTREAMSDK_CPP_VERSION_REPOSITORY "unknown" -#define VICONDATASTREAMSDK_CPP_VERSION_CHANGESET "0" -#define VICONDATASTREAMSDK_CPP_VERSION_LOCAL_CHANGESET "0" -#define VICONDATASTREAMSDK_CPP_VERSION_LOCAL_BRANCH "unknown" - -#define VICONDATASTREAMSDK_CPP_FULL_VERSION_STRING "1.8.0.0" -#define VICONDATASTREAMSDK_CPP_VERSION_STRING "1.8" -#define VICONDATASTREAMSDK_CPP_COMPANY "Vicon Motion Systems Ltd" -#define VICONDATASTREAMSDK_CPP_COPYRIGHT "Copyright \x00A9 2018 Vicon Motion Systems Ltd. All rights reserved." -#define VICONDATASTREAMSDK_CPP_TRADEMARK "Vicon\x00AE is a registered trademark of OMG Plc." -#define VICONDATASTREAMSDK_CPP_PROJECT_NAME "ViconDataStreamSDK_CPP" - -#define VICONDATASTREAMSDK_CPP_FULL_VERSION_STRING_L L"1.8.0.0" -#define VICONDATASTREAMSDK_CPP_VERSION_STRING_L L"1.8" -#define VICONDATASTREAMSDK_CPP_COMPANY_L L"Vicon Motion Systems Ltd" -#define VICONDATASTREAMSDK_CPP_COPYRIGHT_L L"Copyright \x00A9 2018 Vicon Motion Systems Ltd. All rights reserved." -#define VICONDATASTREAMSDK_CPP_TRADEMARK_L L"Vicon\x00AE is a registered trademark of OMG Plc." -#define VICONDATASTREAMSDK_CPP_PROJECT_NAME_L L"ViconDataStreamSDK_CPP" diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/makefile b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/makefile deleted file mode 100644 index d6656479b..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/makefile +++ /dev/null @@ -1,117 +0,0 @@ -# Makefile for ViconDataStreamSDK_CPP - -ifndef VERBOSE -.SILENT : -endif -.SUFFIXES : - -ifdef CONFIG -ifneq ($(CONFIG),Debug) -ifneq ($(CONFIG),InternalRelease) -ifneq ($(CONFIG),Release) -Error: unknown configuration. -endif -endif -endif -else -CONFIG=Debug -endif - -INCLUDEPATHS=. ../../../../thirdparty/Boost/boost-1.58.0-dynamic-linux-x64/installed/include .. ../.. -INCLUDEPATHS_Debug=Debug -INCLUDEPATHS_InternalRelease=InternalRelease -INCLUDEPATHS_Release=Release -LIBRARYPATHS=../../../../thirdparty/Boost/boost-1.58.0-dynamic-linux-x64/installed/lib -LIBRARYPATHS_Debug=../../../../lib/Debug ../../../../bin/Debug -LIBRARYPATHS_InternalRelease=../../../../lib/InternalRelease ../../../../bin/InternalRelease -LIBRARYPATHS_Release=../../../../lib/Release ../../../../bin/Release -DEPENDENCIES=ViconDataStreamSDKCore ViconCGStreamClientSDK ViconCGStreamClient ViconCGStream StreamCommon -LIBRARIES= -LIBRARIES_Debug=boost_wserialization-mt-d boost_wave-mt-d boost_unit_test_framework-mt-d boost_timer-mt-d boost_thread-mt-d boost_system-mt-d boost_signals-mt-d boost_serialization-mt-d boost_regex-mt-d boost_random-mt-d boost_python-mt-d boost_program_options-mt-d boost_prg_exec_monitor-mt-d boost_math_tr1l-mt-d boost_math_tr1f-mt-d boost_math_tr1-mt-d boost_math_c99l-mt-d boost_math_c99f-mt-d boost_math_c99-mt-d boost_log_setup-mt-d boost_log-mt-d boost_locale-mt-d boost_iostreams-mt-d boost_graph-mt-d boost_filesystem-mt-d boost_date_time-mt-d boost_coroutine-mt-d boost_context-mt-d boost_container-mt-d boost_chrono-mt-d boost_atomic-mt-d -LIBRARIES_InternalRelease=boost_wserialization-mt boost_wave-mt boost_unit_test_framework-mt boost_timer-mt boost_thread-mt boost_system-mt boost_signals-mt boost_serialization-mt boost_regex-mt boost_random-mt boost_python-mt boost_program_options-mt boost_prg_exec_monitor-mt boost_math_tr1l-mt boost_math_tr1f-mt boost_math_tr1-mt boost_math_c99l-mt boost_math_c99f-mt boost_math_c99-mt boost_log_setup-mt boost_log-mt boost_locale-mt boost_iostreams-mt boost_graph-mt boost_filesystem-mt boost_date_time-mt boost_coroutine-mt boost_context-mt boost_container-mt boost_chrono-mt boost_atomic-mt -LIBRARIES_Release=boost_wserialization-mt boost_wave-mt boost_unit_test_framework-mt boost_timer-mt boost_thread-mt boost_system-mt boost_signals-mt boost_serialization-mt boost_regex-mt boost_random-mt boost_python-mt boost_program_options-mt boost_prg_exec_monitor-mt boost_math_tr1l-mt boost_math_tr1f-mt boost_math_tr1-mt boost_math_c99l-mt boost_math_c99f-mt boost_math_c99-mt boost_log_setup-mt boost_log-mt boost_locale-mt boost_iostreams-mt boost_graph-mt boost_filesystem-mt boost_date_time-mt boost_coroutine-mt boost_context-mt boost_container-mt boost_chrono-mt boost_atomic-mt -DEFINES=TCM_LINUX TCM_UNIX PROJECT_SOURCE_PATH=\"/home/swtest/workspace/DatastreamSDK_Linux_64_Branch_Release/Binary/bin/Release/deployment/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP\" -DEFINES_Debug=_DEBUG -DEFINES_InternalRelease=VICON_INTERNAL_RELEASE NDEBUG -DEFINES_Release=NDEBUG TCM_OFF_SITE -ENV_CPU=x64 - -SOURCEDIRECTORY=../../../.. -PROJECTPATH=. -BINARYDIRECTORY=../../../.. -INTERMEDIATEDIRECTORY=. -LIBRARYDIRECTORY=../../../../lib -OUTPUTDIRECTORY=../../../../bin - -include $(BINARYDIRECTORY)/gcc.mk - -HIDE_BOOST_SCRIPT=hide_boost_version_script -ifneq ($(HIDE_BOOST),) - HIDE_BOOST_LD_PARAM= -Wl,--version-script=$(HIDE_BOOST_SCRIPT) - HIDE_BOOST_LD_PREREQ=$(HIDE_BOOST_SCRIPT) -endif -all: all_$(CONFIG) - -all_Debug: $(OUTPUTDIRECTORY)/$(CONFIG)/libViconDataStreamSDK_CPP.so -all_InternalRelease: $(OUTPUTDIRECTORY)/$(CONFIG)/libViconDataStreamSDK_CPP.so -all_Release: $(OUTPUTDIRECTORY)/$(CONFIG)/libViconDataStreamSDK_CPP.so - -OBJECTS=$(CONFIG)/DataStreamClient.o $(CONFIG)/DataStreamRetimingClient.o - -CXXFLAGS=$(CXXFLAGS_$(CONFIG)) $(L_INCLUDEPATHS) $(L_INCLUDEPATHS_$(CONFIG)) $(L_INCLUDES) $(L_INCLUDES_$(CONFIG)) $(L_DEFINES) $(L_DEFINES_$(CONFIG)) -CCFLAGS=$(CCFLAGS_$(CONFIG)) $(L_INCLUDEPATHS) $(L_INCLUDEPATHS_$(CONFIG)) $(L_INCLUDES) $(L_INCLUDES_$(CONFIG)) $(L_DEFINES) $(L_DEFINES_$(CONFIG)) -LDFLAGS=$(LDFLAGS_$(CONFIG)) $(L_LIBRARYPATHS) $(L_LIBRARYPATHS_$(CONFIG)) -CXXFLAGS+=-Wno-unused-local-typedefs -CXXFLAGS+=-Wno-delete-non-virtual-dtor -std=c++14 -#Android toolchain does not include librt but integrates some of its functionality into Android libc. -ifndef ANDROID_TARGET_ARCH -LDFLAGS+=-lrt -endif - - -$(OUTPUTDIRECTORY)/Debug/libViconDataStreamSDK_CPP.so: makefile $(OBJECTS) $(LIBRARYDIRECTORY)/$(CONFIG)/libStreamCommon.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStream.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClient.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClientSDK.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconDataStreamSDKCore.a $(HIDE_BOOST_LD_PREREQ) - @echo \[1\;32mLinking dll $@\ - @mkdir -p $(@D) - $(LD) -fPIC -shared -Wl,--as-needed $(LDFLAGS) -o $@ $(OBJECTS) -Wl,--start-group $(L_DEPENDENCIES) $(L_LIBRARIES) $(L_LIBRARIES_$(CONFIG)) -Wl,--end-group -pthread -ldl -Wl,-rpath='$$ORIGIN':$(@D) $(HIDE_BOOST_LD_PARAM) - -$(OUTPUTDIRECTORY)/InternalRelease/libViconDataStreamSDK_CPP.so: makefile $(OBJECTS) $(LIBRARYDIRECTORY)/$(CONFIG)/libStreamCommon.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStream.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClient.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClientSDK.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconDataStreamSDKCore.a $(HIDE_BOOST_LD_PREREQ) - @echo \[1\;32mLinking dll $@\ - @mkdir -p $(@D) - $(LD) -fPIC -shared -Wl,--as-needed $(LDFLAGS) -o $@ $(OBJECTS) -Wl,--start-group $(L_DEPENDENCIES) $(L_LIBRARIES) $(L_LIBRARIES_$(CONFIG)) -Wl,--end-group -pthread -ldl -Wl,-rpath='$$ORIGIN':$(@D) $(HIDE_BOOST_LD_PARAM) - -$(OUTPUTDIRECTORY)/Release/libViconDataStreamSDK_CPP.so: makefile $(OBJECTS) $(LIBRARYDIRECTORY)/$(CONFIG)/libStreamCommon.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStream.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClient.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClientSDK.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconDataStreamSDKCore.a $(HIDE_BOOST_LD_PREREQ) - @echo \[1\;32mLinking dll $@\ - @mkdir -p $(@D) - $(LD) -fPIC -shared -Wl,--as-needed $(LDFLAGS) -o $@ $(OBJECTS) -Wl,--start-group $(L_DEPENDENCIES) $(L_LIBRARIES) $(L_LIBRARIES_$(CONFIG)) -Wl,--end-group -pthread -ldl -Wl,-rpath='$$ORIGIN':$(@D) $(HIDE_BOOST_LD_PARAM) - -# Source Files -$(CONFIG)/DataStreamClient.o: makefile $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/DataStreamClient.cpp - @echo \[1\;34mCompiling DataStreamClient.cpp\ - @mkdir -p $(@D) - find $(CONFIG) -name *.gch -exec cp '{}' . \; - $(CXX) -fPIC -MMD -MP -I$(CONFIG)/ $(CXXFLAGS) -o $@ -c $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/DataStreamClient.cpp - --include $(CONFIG)/DataStreamClient.d - -$(CONFIG)/DataStreamRetimingClient.o: makefile $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/DataStreamRetimingClient.cpp - @echo \[1\;34mCompiling DataStreamRetimingClient.cpp\ - @mkdir -p $(@D) - find $(CONFIG) -name *.gch -exec cp '{}' . \; - $(CXX) -fPIC -MMD -MP -I$(CONFIG)/ $(CXXFLAGS) -o $@ -c $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/DataStreamRetimingClient.cpp - --include $(CONFIG)/DataStreamRetimingClient.d - -# Header Files -# Resource Files -# Other Files - -clean: - @echo \[1\;31mCleaning $(CONFIG) build\ - find . -path '*/$(CONFIG)/*' \( -name '*.[od]' -o -name '*.gch' \) -exec rm -f {} ';' - rm -f moc_*.cxx - -$(HIDE_BOOST_SCRIPT): makefile - echo -n >$@ - echo "{" >>$@ - echo " local: *N5boost*; *NK5boost*;" >>$@ - echo "};" >>$@ diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/resource.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPP/resource.h deleted file mode 100644 index e69de29bb..000000000 diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPPRetimerTest/ViconDataStreamSDK_CPPRetimerTest.cpp b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPPRetimerTest/ViconDataStreamSDK_CPPRetimerTest.cpp deleted file mode 100644 index 15fc4c328..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPPRetimerTest/ViconDataStreamSDK_CPPRetimerTest.cpp +++ /dev/null @@ -1,407 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#include "DataStreamClient.h" -#include "DataStreamRetimingClient.h" - -#include - -#include -#include -#include - -#ifdef WIN32 -#include // For _kbhit() -#include // For getchar() -#include // For Sleep() -#else -#include // For sleep() -#endif // WIN32 - -#define CSV_OUTPUT( FILE, OUTPUT ) if( FILE.good() ) FILE << OUTPUT - -using namespace ViconDataStreamSDK::CPP; - -#ifdef WIN32 -bool Hit() -{ - bool hit = false; - while( _kbhit() ) - { - getchar(); - hit = true; - } - return hit; -} -#endif - -namespace -{ - std::string Adapt(const bool i_Value) - { - return i_Value ? "True" : "False"; - } - - void PrintSubjects(std::ofstream & OutputFile, const ViconDataStreamSDK::CPP::RetimingClient & i_rRetimer) - { - // Count the number of subjects - unsigned int SubjectCount = i_rRetimer.GetSubjectCount().SubjectCount; - std::cout << "Subjects (" << SubjectCount << "):" << std::endl; - for (unsigned int SubjectIndex = 0; SubjectIndex < SubjectCount; ++SubjectIndex) - { - std::cout << " Subject #" << SubjectIndex << std::endl; - - // Get the subject name - std::string SubjectName = i_rRetimer.GetSubjectName(SubjectIndex).SubjectName; - std::cout << " Name: " << SubjectName << std::endl; - - // Get the root segment - std::string RootSegment = i_rRetimer.GetSubjectRootSegmentName(SubjectName).SegmentName; - std::cout << " Root Segment: " << RootSegment << std::endl; - - // Count the number of segments - unsigned int SegmentCount = i_rRetimer.GetSegmentCount(SubjectName).SegmentCount; - std::cout << " Segments (" << SegmentCount << "):" << std::endl; - for (unsigned int SegmentIndex = 0; SegmentIndex < SegmentCount; ++SegmentIndex) - { - std::cout << " Segment #" << SegmentIndex << std::endl; - - // Get the segment name - std::string SegmentName = i_rRetimer.GetSegmentName(SubjectName, SegmentIndex).SegmentName; - std::cout << " Name: " << SegmentName << std::endl; - - // Get the segment parent - std::string SegmentParentName = i_rRetimer.GetSegmentParentName(SubjectName, SegmentName).SegmentName; - std::cout << " Parent: " << SegmentParentName << std::endl; - - // Get the segment's children - unsigned int ChildCount = i_rRetimer.GetSegmentChildCount(SubjectName, SegmentName).SegmentCount; - std::cout << " Children (" << ChildCount << "):" << std::endl; - for (unsigned int ChildIndex = 0; ChildIndex < ChildCount; ++ChildIndex) - { - std::string ChildName = i_rRetimer.GetSegmentChildName(SubjectName, SegmentName, ChildIndex).SegmentName; - std::cout << " " << ChildName << std::endl; - } - - // Get the static segment translation - Output_GetSegmentStaticTranslation _Output_GetSegmentStaticTranslation = - i_rRetimer.GetSegmentStaticTranslation(SubjectName, SegmentName); - std::cout << " Static Translation: (" << _Output_GetSegmentStaticTranslation.Translation[0] << ", " - << _Output_GetSegmentStaticTranslation.Translation[1] << ", " - << _Output_GetSegmentStaticTranslation.Translation[2] << ")" << std::endl; - - // Get the static segment rotation in helical co-ordinates - Output_GetSegmentStaticRotationHelical _Output_GetSegmentStaticRotationHelical = - i_rRetimer.GetSegmentStaticRotationHelical(SubjectName, SegmentName); - std::cout << " Static Rotation Helical: (" << _Output_GetSegmentStaticRotationHelical.Rotation[0] << ", " - << _Output_GetSegmentStaticRotationHelical.Rotation[1] << ", " - << _Output_GetSegmentStaticRotationHelical.Rotation[2] << ")" << std::endl; - - // Get the static segment rotation as a matrix - Output_GetSegmentStaticRotationMatrix _Output_GetSegmentStaticRotationMatrix = - i_rRetimer.GetSegmentStaticRotationMatrix(SubjectName, SegmentName); - std::cout << " Static Rotation Matrix: (" << _Output_GetSegmentStaticRotationMatrix.Rotation[0] << ", " - << _Output_GetSegmentStaticRotationMatrix.Rotation[1] << ", " - << _Output_GetSegmentStaticRotationMatrix.Rotation[2] << ", " - << _Output_GetSegmentStaticRotationMatrix.Rotation[3] << ", " - << _Output_GetSegmentStaticRotationMatrix.Rotation[4] << ", " - << _Output_GetSegmentStaticRotationMatrix.Rotation[5] << ", " - << _Output_GetSegmentStaticRotationMatrix.Rotation[6] << ", " - << _Output_GetSegmentStaticRotationMatrix.Rotation[7] << ", " - << _Output_GetSegmentStaticRotationMatrix.Rotation[8] << ")" << std::endl; - - // Get the static segment rotation in quaternion co-ordinates - Output_GetSegmentStaticRotationQuaternion _Output_GetSegmentStaticRotationQuaternion = - i_rRetimer.GetSegmentStaticRotationQuaternion(SubjectName, SegmentName); - std::cout << " Static Rotation Quaternion: (" << _Output_GetSegmentStaticRotationQuaternion.Rotation[0] << ", " - << _Output_GetSegmentStaticRotationQuaternion.Rotation[1] << ", " - << _Output_GetSegmentStaticRotationQuaternion.Rotation[2] << ", " - << _Output_GetSegmentStaticRotationQuaternion.Rotation[3] << ")" << std::endl; - - // Get the static segment rotation in EulerXYZ co-ordinates - Output_GetSegmentStaticRotationEulerXYZ _Output_GetSegmentStaticRotationEulerXYZ = - i_rRetimer.GetSegmentStaticRotationEulerXYZ(SubjectName, SegmentName); - std::cout << " Static Rotation EulerXYZ: (" << _Output_GetSegmentStaticRotationEulerXYZ.Rotation[0] << ", " - << _Output_GetSegmentStaticRotationEulerXYZ.Rotation[1] << ", " - << _Output_GetSegmentStaticRotationEulerXYZ.Rotation[2] << ")" << std::endl; - - // Get the global segment translation - Output_GetSegmentGlobalTranslation _Output_GetSegmentGlobalTranslation = - i_rRetimer.GetSegmentGlobalTranslation(SubjectName, SegmentName); - std::cout << " Global Translation: (" << _Output_GetSegmentGlobalTranslation.Translation[0] << ", " - << _Output_GetSegmentGlobalTranslation.Translation[1] << ", " - << _Output_GetSegmentGlobalTranslation.Translation[2] << ") " - << Adapt(_Output_GetSegmentGlobalTranslation.Occluded) << std::endl; - - // Get the global segment rotation in helical co-ordinates - Output_GetSegmentGlobalRotationHelical _Output_GetSegmentGlobalRotationHelical = - i_rRetimer.GetSegmentGlobalRotationHelical(SubjectName, SegmentName); - std::cout << " Global Rotation Helical: (" << _Output_GetSegmentGlobalRotationHelical.Rotation[0] << ", " - << _Output_GetSegmentGlobalRotationHelical.Rotation[1] << ", " - << _Output_GetSegmentGlobalRotationHelical.Rotation[2] << ") " - << Adapt(_Output_GetSegmentGlobalRotationHelical.Occluded) << std::endl; - - // Get the global segment rotation as a matrix - Output_GetSegmentGlobalRotationMatrix _Output_GetSegmentGlobalRotationMatrix = - i_rRetimer.GetSegmentGlobalRotationMatrix(SubjectName, SegmentName); - std::cout << " Global Rotation Matrix: (" << _Output_GetSegmentGlobalRotationMatrix.Rotation[0] << ", " - << _Output_GetSegmentGlobalRotationMatrix.Rotation[1] << ", " - << _Output_GetSegmentGlobalRotationMatrix.Rotation[2] << ", " - << _Output_GetSegmentGlobalRotationMatrix.Rotation[3] << ", " - << _Output_GetSegmentGlobalRotationMatrix.Rotation[4] << ", " - << _Output_GetSegmentGlobalRotationMatrix.Rotation[5] << ", " - << _Output_GetSegmentGlobalRotationMatrix.Rotation[6] << ", " - << _Output_GetSegmentGlobalRotationMatrix.Rotation[7] << ", " - << _Output_GetSegmentGlobalRotationMatrix.Rotation[8] << ") " - << Adapt(_Output_GetSegmentGlobalRotationMatrix.Occluded) << std::endl; - - // Get the global segment rotation in quaternion co-ordinates - Output_GetSegmentGlobalRotationQuaternion _Output_GetSegmentGlobalRotationQuaternion = - i_rRetimer.GetSegmentGlobalRotationQuaternion(SubjectName, SegmentName); - std::cout << " Global Rotation Quaternion: (" << _Output_GetSegmentGlobalRotationQuaternion.Rotation[0] << ", " - << _Output_GetSegmentGlobalRotationQuaternion.Rotation[1] << ", " - << _Output_GetSegmentGlobalRotationQuaternion.Rotation[2] << ", " - << _Output_GetSegmentGlobalRotationQuaternion.Rotation[3] << ") " - << Adapt(_Output_GetSegmentGlobalRotationQuaternion.Occluded) << std::endl; - - // Get the global segment rotation in EulerXYZ co-ordinates - Output_GetSegmentGlobalRotationEulerXYZ _Output_GetSegmentGlobalRotationEulerXYZ = - i_rRetimer.GetSegmentGlobalRotationEulerXYZ(SubjectName, SegmentName); - std::cout << " Global Rotation EulerXYZ: (" << _Output_GetSegmentGlobalRotationEulerXYZ.Rotation[0] << ", " - << _Output_GetSegmentGlobalRotationEulerXYZ.Rotation[1] << ", " - << _Output_GetSegmentGlobalRotationEulerXYZ.Rotation[2] << ") " - << Adapt(_Output_GetSegmentGlobalRotationEulerXYZ.Occluded) << std::endl; - - // Get the local segment translation - Output_GetSegmentLocalTranslation _Output_GetSegmentLocalTranslation = - i_rRetimer.GetSegmentLocalTranslation(SubjectName, SegmentName); - std::cout << " Local Translation: (" << _Output_GetSegmentLocalTranslation.Translation[0] << ", " - << _Output_GetSegmentLocalTranslation.Translation[1] << ", " - << _Output_GetSegmentLocalTranslation.Translation[2] << ") " - << Adapt(_Output_GetSegmentLocalTranslation.Occluded) << std::endl; - - // Get the local segment rotation in helical co-ordinates - Output_GetSegmentLocalRotationHelical _Output_GetSegmentLocalRotationHelical = - i_rRetimer.GetSegmentLocalRotationHelical(SubjectName, SegmentName); - std::cout << " Local Rotation Helical: (" << _Output_GetSegmentLocalRotationHelical.Rotation[0] << ", " - << _Output_GetSegmentLocalRotationHelical.Rotation[1] << ", " - << _Output_GetSegmentLocalRotationHelical.Rotation[2] << ") " - << Adapt(_Output_GetSegmentLocalRotationHelical.Occluded) << std::endl; - - // Get the local segment rotation as a matrix - Output_GetSegmentLocalRotationMatrix _Output_GetSegmentLocalRotationMatrix = - i_rRetimer.GetSegmentLocalRotationMatrix(SubjectName, SegmentName); - std::cout << " Local Rotation Matrix: (" << _Output_GetSegmentLocalRotationMatrix.Rotation[0] << ", " - << _Output_GetSegmentLocalRotationMatrix.Rotation[1] << ", " - << _Output_GetSegmentLocalRotationMatrix.Rotation[2] << ", " - << _Output_GetSegmentLocalRotationMatrix.Rotation[3] << ", " - << _Output_GetSegmentLocalRotationMatrix.Rotation[4] << ", " - << _Output_GetSegmentLocalRotationMatrix.Rotation[5] << ", " - << _Output_GetSegmentLocalRotationMatrix.Rotation[6] << ", " - << _Output_GetSegmentLocalRotationMatrix.Rotation[7] << ", " - << _Output_GetSegmentLocalRotationMatrix.Rotation[8] << ") " - << Adapt(_Output_GetSegmentLocalRotationMatrix.Occluded) << std::endl; - - // Get the local segment rotation in quaternion co-ordinates - Output_GetSegmentLocalRotationQuaternion _Output_GetSegmentLocalRotationQuaternion = - i_rRetimer.GetSegmentLocalRotationQuaternion(SubjectName, SegmentName); - std::cout << " Local Rotation Quaternion: (" << _Output_GetSegmentLocalRotationQuaternion.Rotation[0] << ", " - << _Output_GetSegmentLocalRotationQuaternion.Rotation[1] << ", " - << _Output_GetSegmentLocalRotationQuaternion.Rotation[2] << ", " - << _Output_GetSegmentLocalRotationQuaternion.Rotation[3] << ") " - << Adapt(_Output_GetSegmentLocalRotationQuaternion.Occluded) << std::endl; - - // Get the local segment rotation in EulerXYZ co-ordinates - Output_GetSegmentLocalRotationEulerXYZ _Output_GetSegmentLocalRotationEulerXYZ = - i_rRetimer.GetSegmentLocalRotationEulerXYZ(SubjectName, SegmentName); - std::cout << " Local Rotation EulerXYZ: (" << _Output_GetSegmentLocalRotationEulerXYZ.Rotation[0] << ", " - << _Output_GetSegmentLocalRotationEulerXYZ.Rotation[1] << ", " - << _Output_GetSegmentLocalRotationEulerXYZ.Rotation[2] << ") " - << Adapt(_Output_GetSegmentLocalRotationEulerXYZ.Occluded) << std::endl; - } - } - } -} - -int main( int argc, char* argv[] ) -{ - std::cout << "DSSDK C++ API Retiming Test" << std::endl; - - // Program options - - bool bOutput = true; - - std::string HostName = "localhost:801"; - if( argc > 1 ) - { - HostName = argv[1]; - } - - double FrameRate = -1; - std::ofstream OutputFile; - unsigned int NumRestarts = 1; - double Duration = 0; - - for( int a = 2; a < argc; ++a ) - { - std::string arg = argv[a]; - if( arg == "--help" ) - { - std::cout << argv[0] << " : allowed options include:\n --framerate --output --restarts --duration --no-output" << std::endl; - return 0; - } - else if( arg == "--framerate" ) - { - if( a < argc ) - { - FrameRate = boost::lexical_cast( argv[++a] ); - } - } - else if( arg == "--output" ) - { - if( a < argc ) - { - std::string OutputFileName = argv[++a]; - OutputFile.open( OutputFileName ); - if( OutputFile.good() ) - { - std::cout << "Using output file " << OutputFileName << std::endl; - } - else - { - std::cerr << "Failed to open " << OutputFileName << " for output"; - } - } - } - else if( arg == "--restarts" ) - { - if( a < argc ) - { - try - { - NumRestarts = boost::lexical_cast( argv[++a] ) + 1; - } - catch( boost::bad_lexical_cast & e ) - { - std::cerr << e.what() << std::endl; - } - } - } - else if( arg == "--duration" ) - { - if( a < argc ) - { - try - { - Duration = boost::lexical_cast( argv[++a] ); - } - catch( boost::bad_lexical_cast & e ) - { - std::cerr << e.what() << std::endl; - } - } - } - else if( arg == "--no-output" ) - { - bOutput = false; - } - } - - CSV_OUTPUT( OutputFile, "Output File" << std::endl ); - - std::string LogFile = ""; - - ViconDataStreamSDK::CPP::RetimingClient Retimer; - - for (unsigned int RunNumber = 0; RunNumber < NumRestarts; ++RunNumber) - { - if (NumRestarts > 1) - { - std::cout << "Run number " << RunNumber << " with duration " << Duration << std::endl; - } - - if (Retimer.Connect(HostName, FrameRate ).Result != Result::Success) - { - std::cout << "Could not connect to " << HostName << std::endl; - return false; - } - - const auto StartTime = std::chrono::high_resolution_clock::now(); - bool bStop = false; - - // The retimer may sometimes take a few frames to start producing data as it - // needs to fill a buffer before it can produce output. - // We will use this flag to inhibit output during this spool up phase. - bool bDataReceived = false; - -#ifdef WIN32 - while( !bStop && !Hit() ) -#else - while( !bStop ) -#endif - { - - - if( Duration != 0 ) - { - double TimeElapsed = static_cast< double >( std::chrono::duration_cast( - ( std::chrono::high_resolution_clock::now() - StartTime ) ).count() ); - bStop = TimeElapsed > Duration; - } - - // We are running on a loop timer - use this method if you do not have an existing timer in your project - if( FrameRate > 0 ) - { - // Wait for a frame. This method blocks until the next frame period has elapsed. - auto WaitResult = Retimer.WaitForFrame(); - bDataReceived = bDataReceived || ( WaitResult.Result == Result::Success ); - } - else - { - auto UpdateResult = Retimer.UpdateFrame(); - bDataReceived = bDataReceived || (UpdateResult.Result == Result::Success); - - // Impose our own simple frame rate throttle, so that we do not overload the process -#ifdef WIN32 - Sleep( 10 ); -#else - sleep( 1 ); -#endif - } - - if (bDataReceived && bOutput) - { - PrintSubjects(OutputFile, Retimer); - } - } - - Retimer.Disconnect(); - } - - if( OutputFile.good() ) - { - OutputFile.close(); - } -} - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPPRetimerTest/makefile b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPPRetimerTest/makefile deleted file mode 100644 index 16a9ea71f..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPPRetimerTest/makefile +++ /dev/null @@ -1,107 +0,0 @@ -# Makefile for ViconDataStreamSDK_CPPRetimerTest - -ifndef VERBOSE -.SILENT : -endif -.SUFFIXES : - -ifdef CONFIG -ifneq ($(CONFIG),Debug) -ifneq ($(CONFIG),InternalRelease) -ifneq ($(CONFIG),Release) -Error: unknown configuration. -endif -endif -endif -else -CONFIG=Debug -endif - -INCLUDEPATHS=../../../../thirdparty/Boost/boost-1.58.0-dynamic-linux-x64/installed/include . ../ViconDataStreamSDK_CPP ../.. .. -INCLUDEPATHS_Debug=Debug -INCLUDEPATHS_InternalRelease=InternalRelease -INCLUDEPATHS_Release=Release -LIBRARYPATHS=../../../../thirdparty/Boost/boost-1.58.0-dynamic-linux-x64/installed/lib -LIBRARYPATHS_Debug=../../../../lib/Debug ../../../../bin/Debug -LIBRARYPATHS_InternalRelease=../../../../lib/InternalRelease ../../../../bin/InternalRelease -LIBRARYPATHS_Release=../../../../lib/Release ../../../../bin/Release -DEPENDENCIES=ViconDataStreamSDKCore ViconDataStreamSDK_CPP ViconCGStreamClientSDK ViconCGStreamClient ViconCGStream StreamCommon -LIBRARIES= -LIBRARIES_Debug=boost_wserialization-mt-d boost_wave-mt-d boost_unit_test_framework-mt-d boost_timer-mt-d boost_thread-mt-d boost_system-mt-d boost_signals-mt-d boost_serialization-mt-d boost_regex-mt-d boost_random-mt-d boost_python-mt-d boost_program_options-mt-d boost_prg_exec_monitor-mt-d boost_math_tr1l-mt-d boost_math_tr1f-mt-d boost_math_tr1-mt-d boost_math_c99l-mt-d boost_math_c99f-mt-d boost_math_c99-mt-d boost_log_setup-mt-d boost_log-mt-d boost_locale-mt-d boost_iostreams-mt-d boost_graph-mt-d boost_filesystem-mt-d boost_date_time-mt-d boost_coroutine-mt-d boost_context-mt-d boost_container-mt-d boost_chrono-mt-d boost_atomic-mt-d -LIBRARIES_InternalRelease=boost_wserialization-mt boost_wave-mt boost_unit_test_framework-mt boost_timer-mt boost_thread-mt boost_system-mt boost_signals-mt boost_serialization-mt boost_regex-mt boost_random-mt boost_python-mt boost_program_options-mt boost_prg_exec_monitor-mt boost_math_tr1l-mt boost_math_tr1f-mt boost_math_tr1-mt boost_math_c99l-mt boost_math_c99f-mt boost_math_c99-mt boost_log_setup-mt boost_log-mt boost_locale-mt boost_iostreams-mt boost_graph-mt boost_filesystem-mt boost_date_time-mt boost_coroutine-mt boost_context-mt boost_container-mt boost_chrono-mt boost_atomic-mt -LIBRARIES_Release=boost_wserialization-mt boost_wave-mt boost_unit_test_framework-mt boost_timer-mt boost_thread-mt boost_system-mt boost_signals-mt boost_serialization-mt boost_regex-mt boost_random-mt boost_python-mt boost_program_options-mt boost_prg_exec_monitor-mt boost_math_tr1l-mt boost_math_tr1f-mt boost_math_tr1-mt boost_math_c99l-mt boost_math_c99f-mt boost_math_c99-mt boost_log_setup-mt boost_log-mt boost_locale-mt boost_iostreams-mt boost_graph-mt boost_filesystem-mt boost_date_time-mt boost_coroutine-mt boost_context-mt boost_container-mt boost_chrono-mt boost_atomic-mt -DEFINES=TCM_LINUX TCM_UNIX PROJECT_SOURCE_PATH=\"/home/swtest/workspace/DatastreamSDK_Linux_64_Branch_Release/Binary/bin/Release/deployment/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPPRetimerTest\" -DEFINES_Debug=_DEBUG -DEFINES_InternalRelease=VICON_INTERNAL_RELEASE NDEBUG -DEFINES_Release=NDEBUG TCM_OFF_SITE -ENV_CPU=x64 - -SOURCEDIRECTORY=../../../.. -PROJECTPATH=. -BINARYDIRECTORY=../../../.. -INTERMEDIATEDIRECTORY=. -LIBRARYDIRECTORY=../../../../lib -OUTPUTDIRECTORY=../../../../bin - -include $(BINARYDIRECTORY)/gcc.mk - -HIDE_BOOST_SCRIPT=hide_boost_version_script -ifneq ($(HIDE_BOOST),) - HIDE_BOOST_LD_PARAM= -Wl,--version-script=$(HIDE_BOOST_SCRIPT) - HIDE_BOOST_LD_PREREQ=$(HIDE_BOOST_SCRIPT) -endif -all: all_$(CONFIG) - -all_Debug: $(OUTPUTDIRECTORY)/$(CONFIG)/ViconDataStreamSDK_CPPRetimerTest -all_InternalRelease: $(OUTPUTDIRECTORY)/$(CONFIG)/ViconDataStreamSDK_CPPRetimerTest -all_Release: $(OUTPUTDIRECTORY)/$(CONFIG)/ViconDataStreamSDK_CPPRetimerTest - -OBJECTS=$(CONFIG)/ViconDataStreamSDK_CPPRetimerTest.o - -CXXFLAGS=$(CXXFLAGS_$(CONFIG)) $(L_INCLUDEPATHS) $(L_INCLUDEPATHS_$(CONFIG)) $(L_INCLUDES) $(L_INCLUDES_$(CONFIG)) $(L_DEFINES) $(L_DEFINES_$(CONFIG)) -CCFLAGS=$(CCFLAGS_$(CONFIG)) $(L_INCLUDEPATHS) $(L_INCLUDEPATHS_$(CONFIG)) $(L_INCLUDES) $(L_INCLUDES_$(CONFIG)) $(L_DEFINES) $(L_DEFINES_$(CONFIG)) -LDFLAGS=$(LDFLAGS_$(CONFIG)) $(L_LIBRARYPATHS) $(L_LIBRARYPATHS_$(CONFIG)) -CXXFLAGS+=-Wno-unused-local-typedefs -CXXFLAGS+=-Wno-delete-non-virtual-dtor -std=c++14 -#Android toolchain does not include librt but integrates some of its functionality into Android libc. -ifndef ANDROID_TARGET_ARCH -LDFLAGS+=-lrt -endif - - -$(OUTPUTDIRECTORY)/Debug/ViconDataStreamSDK_CPPRetimerTest: makefile $(OBJECTS) $(LIBRARYDIRECTORY)/$(CONFIG)/libStreamCommon.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStream.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClient.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClientSDK.a $(OUTPUTDIRECTORY)/$(CONFIG)/libViconDataStreamSDK_CPP.so $(LIBRARYDIRECTORY)/$(CONFIG)/libViconDataStreamSDKCore.a - @echo \[1\;32mLinking EXE $@\ - @mkdir -p $(@D) - $(LD) -Wl,--as-needed -export-dynamic $(LDFLAGS) -o $@ $(OBJECTS) -Wl,--start-group $(L_DEPENDENCIES) $(L_LIBRARIES_$(CONFIG)) $(L_LIBRARIES) -Wl,--end-group -pthread -ldl -Wl,-rpath='$$ORIGIN' -Wl,-rpath-link=.:$(@D) - -$(OUTPUTDIRECTORY)/InternalRelease/ViconDataStreamSDK_CPPRetimerTest: makefile $(OBJECTS) $(LIBRARYDIRECTORY)/$(CONFIG)/libStreamCommon.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStream.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClient.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClientSDK.a $(OUTPUTDIRECTORY)/$(CONFIG)/libViconDataStreamSDK_CPP.so $(LIBRARYDIRECTORY)/$(CONFIG)/libViconDataStreamSDKCore.a - @echo \[1\;32mLinking EXE $@\ - @mkdir -p $(@D) - $(LD) -Wl,--as-needed -export-dynamic $(LDFLAGS) -o $@ $(OBJECTS) -Wl,--start-group $(L_DEPENDENCIES) $(L_LIBRARIES_$(CONFIG)) $(L_LIBRARIES) -Wl,--end-group -pthread -ldl -Wl,-rpath='$$ORIGIN' -Wl,-rpath-link=.:$(@D) - -$(OUTPUTDIRECTORY)/Release/ViconDataStreamSDK_CPPRetimerTest: makefile $(OBJECTS) $(LIBRARYDIRECTORY)/$(CONFIG)/libStreamCommon.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStream.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClient.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClientSDK.a $(OUTPUTDIRECTORY)/$(CONFIG)/libViconDataStreamSDK_CPP.so $(LIBRARYDIRECTORY)/$(CONFIG)/libViconDataStreamSDKCore.a - @echo \[1\;32mLinking EXE $@\ - @mkdir -p $(@D) - $(LD) -Wl,--as-needed -export-dynamic $(LDFLAGS) -o $@ $(OBJECTS) -Wl,--start-group $(L_DEPENDENCIES) $(L_LIBRARIES_$(CONFIG)) $(L_LIBRARIES) -Wl,--end-group -pthread -ldl -Wl,-rpath='$$ORIGIN' -Wl,-rpath-link=.:$(@D) - -# Source Files -$(CONFIG)/ViconDataStreamSDK_CPPRetimerTest.o: makefile $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPPRetimerTest/ViconDataStreamSDK_CPPRetimerTest.cpp - @echo \[1\;34mCompiling ViconDataStreamSDK_CPPRetimerTest.cpp\ - @mkdir -p $(@D) - find $(CONFIG) -name *.gch -exec cp '{}' . \; - $(CXX) -fPIC -MMD -MP -I$(CONFIG)/ $(CXXFLAGS) -o $@ -c $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPPRetimerTest/ViconDataStreamSDK_CPPRetimerTest.cpp - --include $(CONFIG)/ViconDataStreamSDK_CPPRetimerTest.d - -# Other Files - -clean: - @echo \[1\;31mCleaning $(CONFIG) build\ - find . -path '*/$(CONFIG)/*' \( -name '*.[od]' -o -name '*.gch' \) -exec rm -f {} ';' - rm -f moc_*.cxx - -$(HIDE_BOOST_SCRIPT): makefile - echo -n >$@ - echo "{" >>$@ - echo " local: *N5boost*; *NK5boost*;" >>$@ - echo "};" >>$@ diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPPTest/ViconDataStreamSDK_CPPTest.cpp b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPPTest/ViconDataStreamSDK_CPPTest.cpp deleted file mode 100644 index ae37ee2dd..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPPTest/ViconDataStreamSDK_CPPTest.cpp +++ /dev/null @@ -1,1056 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// - -#include "DataStreamClient.h" - -#include -#include -#include -#include -#include -#include -#include - -#ifdef WIN32 - #include // For _kbhit() - #include // For getchar() - #include // For Sleep() -#else - #include // For sleep() -#endif // WIN32 - -#include - -using namespace ViconDataStreamSDK::CPP; - -#define output_stream if(!LogFile.empty()) ; else std::cout - -namespace -{ - std::string Adapt( const bool i_Value ) - { - return i_Value ? "True" : "False"; - } - - std::string Adapt( const TimecodeStandard::Enum i_Standard ) - { - switch( i_Standard ) - { - default: - case TimecodeStandard::None: - return "0"; - case TimecodeStandard::PAL: - return "1"; - case TimecodeStandard::NTSC: - return "2"; - case TimecodeStandard::NTSCDrop: - return "3"; - case TimecodeStandard::Film: - return "4"; - case TimecodeStandard::NTSCFilm: - return "5"; - case TimecodeStandard::ATSC: - return "6"; - } - } - - std::string Adapt( const Direction::Enum i_Direction ) - { - switch( i_Direction ) - { - case Direction::Forward: - return "Forward"; - case Direction::Backward: - return "Backward"; - case Direction::Left: - return "Left"; - case Direction::Right: - return "Right"; - case Direction::Up: - return "Up"; - case Direction::Down: - return "Down"; - default: - return "Unknown"; - } - } - - std::string Adapt( const DeviceType::Enum i_DeviceType ) - { - switch( i_DeviceType ) - { - case DeviceType::ForcePlate: - return "ForcePlate"; - case DeviceType::Unknown: - default: - return "Unknown"; - } - } - - std::string Adapt( const Unit::Enum i_Unit ) - { - switch( i_Unit ) - { - case Unit::Meter: - return "Meter"; - case Unit::Volt: - return "Volt"; - case Unit::NewtonMeter: - return "NewtonMeter"; - case Unit::Newton: - return "Newton"; - case Unit::Kilogram: - return "Kilogram"; - case Unit::Second: - return "Second"; - case Unit::Ampere: - return "Ampere"; - case Unit::Kelvin: - return "Kelvin"; - case Unit::Mole: - return "Mole"; - case Unit::Candela: - return "Candela"; - case Unit::Radian: - return "Radian"; - case Unit::Steradian: - return "Steradian"; - case Unit::MeterSquared: - return "MeterSquared"; - case Unit::MeterCubed: - return "MeterCubed"; - case Unit::MeterPerSecond: - return "MeterPerSecond"; - case Unit::MeterPerSecondSquared: - return "MeterPerSecondSquared"; - case Unit::RadianPerSecond: - return "RadianPerSecond"; - case Unit::RadianPerSecondSquared: - return "RadianPerSecondSquared"; - case Unit::Hertz: - return "Hertz"; - case Unit::Joule: - return "Joule"; - case Unit::Watt: - return "Watt"; - case Unit::Pascal: - return "Pascal"; - case Unit::Lumen: - return "Lumen"; - case Unit::Lux: - return "Lux"; - case Unit::Coulomb: - return "Coulomb"; - case Unit::Ohm: - return "Ohm"; - case Unit::Farad: - return "Farad"; - case Unit::Weber: - return "Weber"; - case Unit::Tesla: - return "Tesla"; - case Unit::Henry: - return "Henry"; - case Unit::Siemens: - return "Siemens"; - case Unit::Becquerel: - return "Becquerel"; - case Unit::Gray: - return "Gray"; - case Unit::Sievert: - return "Sievert"; - case Unit::Katal: - return "Katal"; - - case Unit::Unknown: - default: - return "Unknown"; - } - } -#ifdef WIN32 - bool Hit() - { - bool hit = false; - while( _kbhit() ) - { - getchar(); - hit = true; - } - return hit; - } -#endif -} - -int main( int argc, char* argv[] ) -{ - // Program options - - std::string HostName = "localhost:801"; - if( argc > 1 ) - { - HostName = argv[1]; - } - - // log contains: - // version number - // log of framerate over time - // --multicast - // kill off internal app - std::string LogFile = ""; - std::string MulticastAddress = "244.0.0.0:44801"; - bool ConnectToMultiCast = false; - bool EnableMultiCast = false; - bool EnableHapticTest = false; - bool bReadCentroids = false; - bool bReadRayData = false; - bool bReadGreyscaleData = false; - bool bReadVideoData = false; - - std::vector HapticOnList(0); - unsigned int ClientBufferSize = 0; - std::string AxisMapping = "ZUp"; - - for(int a=2; a < argc; ++a) - { - std::string arg = argv[a]; - if(arg == "--help") - { - std::cout << argv[0] << " : allowed options include:\n --log_file --enable_multicast --connect_to_multicast --help --enable_haptic_test --centroids --client-buffer-size " << std::endl; - return 0; - } - else if (arg=="--log_file") - { - if(a < argc) - { - LogFile = argv[a+1]; - std::cout << "Using log file <"<< LogFile << "> ..." << std::endl; - ++a; - } - } - else if (arg=="--enable_multicast") - { - EnableMultiCast = true; - if(a < argc) - { - MulticastAddress = argv[a+1]; - std::cout << "Enabling multicast address <"<< MulticastAddress << "> ..." << std::endl; - ++a; - } - } - else if (arg=="--connect_to_multicast") - { - ConnectToMultiCast = true; - if(a < argc) - { - MulticastAddress = argv[a+1]; - std::cout << "connecting to multicast address <"<< MulticastAddress << "> ..." << std::endl; - ++a; - } - } - else if (arg=="--enable_haptic_test") - { - EnableHapticTest = true; - ++a; - if ( a < argc ) - { - //assuming no haptic device name starts with "--" - while( a < argc && strncmp( argv[a], "--", 2 ) !=0 ) - { - HapticOnList.push_back( argv[a] ); - ++a; - } - } - } - else if( arg=="--centroids" ) - { - bReadCentroids = true; - } - else if( arg=="--rays") - { - bReadRayData = true; - } - else if (arg == "--greyscale") - { - bReadGreyscaleData = true; - bReadCentroids = true; - output_stream << "Enabling greyscale data also enables centroid output" << std::endl; - } - else if (arg == "--video") - { - bReadVideoData = true; - bReadCentroids = true; - output_stream << "Enabling video data also enables centroid output" << std::endl; - } - else if( arg == "--client-buffer-size" ) - { - ++a; - if( a < argc ) - { - ClientBufferSize = atoi( argv[a] ); - } - } - else if( arg == "--set-axis-mapping" ) - { - ++a; - if( a < argc ) - { - AxisMapping = argv[a] ; - - if( AxisMapping == "XUp" || AxisMapping == "YUp" || AxisMapping == "ZUp" ) - { - std::cout << "Setting Axis to "<< AxisMapping << std::endl; - } - else - { - std::cout << "Unknown axis setting: "<< AxisMapping <<" . Should be XUp, YUp, or ZUp" << std::endl; - return 1; - } - } - } - else - { - std::cout << "Failed to understand argument <" << argv[a] << ">...exiting" << std::endl; - return 1; - } - } - - std::ofstream ofs; - if(!LogFile.empty()) - { - ofs.open(LogFile.c_str()); - if(!ofs.is_open()) - { - std::cout << "Could not open log file <" << LogFile << ">...exiting" << std::endl; - return 1; - } - } - // Make a new client - ViconDataStreamSDK::CPP::Client MyClient; - - for(int i=0; i != 3; ++i) // repeat to check disconnecting doesn't wreck next connect - { - // Connect to a server - std::cout << "Connecting to " << HostName << " ..." << std::flush; - while( !MyClient.IsConnected().Connected ) - { - // Direct connection - - bool ok = false; - if(ConnectToMultiCast) - { - // Multicast connection - ok = ( MyClient.ConnectToMulticast( HostName, MulticastAddress ).Result == Result::Success ); - - } - else - { - ok =( MyClient.Connect( HostName ).Result == Result::Success ); - } - if(!ok) - { - std::cout << "Warning - connect failed..." << std::endl; - } - - - std::cout << "."; - #ifdef WIN32 - Sleep( 1000 ); - #else - sleep(1); - #endif - } - std::cout << std::endl; - - // Enable some different data types - MyClient.EnableSegmentData(); - MyClient.EnableMarkerData(); - MyClient.EnableUnlabeledMarkerData(); - MyClient.EnableMarkerRayData(); - MyClient.EnableDeviceData(); - MyClient.EnableDebugData(); - if( bReadCentroids ) - { - MyClient.EnableCentroidData(); - } - if( bReadRayData ) - { - MyClient.EnableMarkerRayData(); - } - if (bReadGreyscaleData) - { - MyClient.EnableGreyscaleData(); - } - if (bReadVideoData) - { - MyClient.EnableVideoData(); - } - - std::cout << "Segment Data Enabled: " << Adapt( MyClient.IsSegmentDataEnabled().Enabled ) << std::endl; - std::cout << "Marker Data Enabled: " << Adapt( MyClient.IsMarkerDataEnabled().Enabled ) << std::endl; - std::cout << "Unlabeled Marker Data Enabled: " << Adapt( MyClient.IsUnlabeledMarkerDataEnabled().Enabled ) << std::endl; - std::cout << "Device Data Enabled: " << Adapt( MyClient.IsDeviceDataEnabled().Enabled ) << std::endl; - std::cout << "Centroid Data Enabled: " << Adapt( MyClient.IsCentroidDataEnabled().Enabled ) << std::endl; - std::cout << "Marker Ray Data Enabled: " << Adapt( MyClient.IsMarkerRayDataEnabled().Enabled ) << std::endl; - std::cout << "Centroid Data Enabled: " << Adapt( MyClient.IsCentroidDataEnabled().Enabled) << std::endl; - std::cout << "Greyscale Data Enabled: " << Adapt( MyClient.IsGreyscaleDataEnabled().Enabled) << std::endl; - std::cout << "Video Data Enabled: " << Adapt( MyClient.IsVideoDataEnabled().Enabled) << std::endl; - std::cout << "Debug Data Enabled: " << Adapt( MyClient.IsDebugDataEnabled().Enabled) << std::endl; - - // Set the streaming mode - //MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ClientPull ); - // MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ClientPullPreFetch ); - MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ServerPush ); - - // Set the global up axis - MyClient.SetAxisMapping( Direction::Forward, - Direction::Left, - Direction::Up ); // Z-up - - if( AxisMapping == "YUp") - { - MyClient.SetAxisMapping( Direction::Forward, - Direction::Up, - Direction::Right ); // Y-up - } - else if( AxisMapping == "XUp") - { - MyClient.SetAxisMapping( Direction::Up, - Direction::Forward, - Direction::Left ); // Y-up - } - - Output_GetAxisMapping _Output_GetAxisMapping = MyClient.GetAxisMapping(); - std::cout << "Axis Mapping: X-" << Adapt( _Output_GetAxisMapping.XAxis ) - << " Y-" << Adapt( _Output_GetAxisMapping.YAxis ) - << " Z-" << Adapt( _Output_GetAxisMapping.ZAxis ) << std::endl; - - // Discover the version number - Output_GetVersion _Output_GetVersion = MyClient.GetVersion(); - std::cout << "Version: " << _Output_GetVersion.Major << "." - << _Output_GetVersion.Minor << "." - << _Output_GetVersion.Point << std::endl; - - if( ClientBufferSize > 0 ) - { - MyClient.SetBufferSize( ClientBufferSize ); - std::cout << "Setting client buffer size to " << ClientBufferSize << std::endl; - } - - if( EnableMultiCast ) - { - assert( MyClient.IsConnected().Connected ); - MyClient.StartTransmittingMulticast( HostName, MulticastAddress ); - } - - size_t FrameRateWindow = 1000; // frames - size_t Counter = 0; - clock_t LastTime = clock(); - // Loop until a key is pressed - #ifdef WIN32 - while( !Hit() ) - #else - while( true) - #endif - { - // Get a frame - output_stream << "Waiting for new frame..."; - while( MyClient.GetFrame().Result != Result::Success ) - { - // Sleep a little so that we don't lumber the CPU with a busy poll - #ifdef WIN32 - Sleep( 200 ); - #else - sleep(1); - #endif - - output_stream << "."; - } - output_stream << std::endl; - if(++Counter == FrameRateWindow) - { - clock_t Now = clock(); - double FrameRate = (double)(FrameRateWindow * CLOCKS_PER_SEC) / (double)(Now - LastTime); - if(!LogFile.empty()) - { - time_t rawtime; - struct tm * timeinfo; - time ( &rawtime ); - timeinfo = localtime ( &rawtime ); - - ofs << "Frame rate = " << FrameRate << " at " << asctime (timeinfo)<< std::endl; - } - - LastTime = Now; - Counter = 0; - } - - // Get the frame number - Output_GetFrameNumber _Output_GetFrameNumber = MyClient.GetFrameNumber(); - output_stream << "Frame Number: " << _Output_GetFrameNumber.FrameNumber << std::endl; - - Output_GetFrameRate Rate = MyClient.GetFrameRate(); - std::cout << "Frame rate: " << Rate.FrameRateHz << std::endl; - - // Show frame rates - for( unsigned int FramerateIndex = 0 ; FramerateIndex < MyClient.GetFrameRateCount().Count ; ++FramerateIndex ) - { - std::string FramerateName = MyClient.GetFrameRateName( FramerateIndex ).Name; - double FramerateValue = MyClient.GetFrameRateValue( FramerateName ).Value; - - output_stream << FramerateName << ": " << FramerateValue << "Hz" << std::endl; - } - output_stream << std::endl; - - // Get the timecode - Output_GetTimecode _Output_GetTimecode = MyClient.GetTimecode(); - - output_stream << "Timecode: " - << _Output_GetTimecode.Hours << "h " - << _Output_GetTimecode.Minutes << "m " - << _Output_GetTimecode.Seconds << "s " - << _Output_GetTimecode.Frames << "f " - << _Output_GetTimecode.SubFrame << "sf " - << Adapt( _Output_GetTimecode.FieldFlag ) << " " - << Adapt( _Output_GetTimecode.Standard ) << " " - << _Output_GetTimecode.SubFramesPerFrame << " " - << _Output_GetTimecode.UserBits << std::endl << std::endl; - - // Get the latency - output_stream << "Latency: " << MyClient.GetLatencyTotal().Total << "s" << std::endl; - - for( unsigned int LatencySampleIndex = 0 ; LatencySampleIndex < MyClient.GetLatencySampleCount().Count ; ++LatencySampleIndex ) - { - std::string SampleName = MyClient.GetLatencySampleName( LatencySampleIndex ).Name; - double SampleValue = MyClient.GetLatencySampleValue( SampleName ).Value; - - output_stream << " " << SampleName << " " << SampleValue << "s" << std::endl; - } - output_stream << std::endl; - - Output_GetHardwareFrameNumber _Output_GetHardwareFrameNumber = MyClient.GetHardwareFrameNumber(); - output_stream << "Hardware Frame Number: " << _Output_GetHardwareFrameNumber.HardwareFrameNumber << std::endl; - - if (EnableHapticTest == true) - { - for (size_t h = 0; h < HapticOnList.size(); ++h) - { - if (Counter % 2 == 0) - { - Output_SetApexDeviceFeedback Output = MyClient.SetApexDeviceFeedback(HapticOnList[h], true); - if (Output.Result == Result::Success) - { - output_stream << "Turn haptic feedback on for device: " << HapticOnList[h] << std::endl; - } - else if (Output.Result == Result::InvalidDeviceName) - { - output_stream << "Device doesn't exist: " << HapticOnList[h] << std::endl; - } - } - if (Counter % 20 == 0) - { - Output_SetApexDeviceFeedback Output = MyClient.SetApexDeviceFeedback(HapticOnList[h], false); - - if (Output.Result == Result::Success) - { - output_stream << "Turn haptic feedback off for device: " << HapticOnList[h] << std::endl; - } - } - } - } - - // Count the number of subjects - unsigned int SubjectCount = MyClient.GetSubjectCount().SubjectCount; - output_stream << "Subjects (" << SubjectCount << "):" << std::endl; - for( unsigned int SubjectIndex = 0 ; SubjectIndex < SubjectCount ; ++SubjectIndex ) - { - output_stream << " Subject #" << SubjectIndex << std::endl; - - // Get the subject name - std::string SubjectName = MyClient.GetSubjectName( SubjectIndex ).SubjectName; - output_stream << " Name: " << SubjectName << std::endl; - - // Get the root segment - std::string RootSegment = MyClient.GetSubjectRootSegmentName( SubjectName ).SegmentName; - output_stream << " Root Segment: " << RootSegment << std::endl; - - // Count the number of segments - unsigned int SegmentCount = MyClient.GetSegmentCount( SubjectName ).SegmentCount; - output_stream << " Segments (" << SegmentCount << "):" << std::endl; - for( unsigned int SegmentIndex = 0 ; SegmentIndex < SegmentCount ; ++SegmentIndex ) - { - output_stream << " Segment #" << SegmentIndex << std::endl; - - // Get the segment name - std::string SegmentName = MyClient.GetSegmentName( SubjectName, SegmentIndex ).SegmentName; - output_stream << " Name: " << SegmentName << std::endl; - - // Get the segment parent - std::string SegmentParentName = MyClient.GetSegmentParentName( SubjectName, SegmentName ).SegmentName; - output_stream << " Parent: " << SegmentParentName << std::endl; - - // Get the segment's children - unsigned int ChildCount = MyClient.GetSegmentChildCount( SubjectName, SegmentName ).SegmentCount; - output_stream << " Children (" << ChildCount << "):" << std::endl; - for( unsigned int ChildIndex = 0 ; ChildIndex < ChildCount ; ++ChildIndex ) - { - std::string ChildName = MyClient.GetSegmentChildName( SubjectName, SegmentName, ChildIndex ).SegmentName; - output_stream << " " << ChildName << std::endl; - } - - // Get the static segment translation - Output_GetSegmentStaticTranslation _Output_GetSegmentStaticTranslation = - MyClient.GetSegmentStaticTranslation( SubjectName, SegmentName ); - output_stream << " Static Translation: (" << _Output_GetSegmentStaticTranslation.Translation[ 0 ] << ", " - << _Output_GetSegmentStaticTranslation.Translation[ 1 ] << ", " - << _Output_GetSegmentStaticTranslation.Translation[ 2 ] << ")" << std::endl; - - // Get the static segment rotation in helical co-ordinates - Output_GetSegmentStaticRotationHelical _Output_GetSegmentStaticRotationHelical = - MyClient.GetSegmentStaticRotationHelical( SubjectName, SegmentName ); - output_stream << " Static Rotation Helical: (" << _Output_GetSegmentStaticRotationHelical.Rotation[ 0 ] << ", " - << _Output_GetSegmentStaticRotationHelical.Rotation[ 1 ] << ", " - << _Output_GetSegmentStaticRotationHelical.Rotation[ 2 ] << ")" << std::endl; - - // Get the static segment rotation as a matrix - Output_GetSegmentStaticRotationMatrix _Output_GetSegmentStaticRotationMatrix = - MyClient.GetSegmentStaticRotationMatrix( SubjectName, SegmentName ); - output_stream << " Static Rotation Matrix: (" << _Output_GetSegmentStaticRotationMatrix.Rotation[ 0 ] << ", " - << _Output_GetSegmentStaticRotationMatrix.Rotation[ 1 ] << ", " - << _Output_GetSegmentStaticRotationMatrix.Rotation[ 2 ] << ", " - << _Output_GetSegmentStaticRotationMatrix.Rotation[ 3 ] << ", " - << _Output_GetSegmentStaticRotationMatrix.Rotation[ 4 ] << ", " - << _Output_GetSegmentStaticRotationMatrix.Rotation[ 5 ] << ", " - << _Output_GetSegmentStaticRotationMatrix.Rotation[ 6 ] << ", " - << _Output_GetSegmentStaticRotationMatrix.Rotation[ 7 ] << ", " - << _Output_GetSegmentStaticRotationMatrix.Rotation[ 8 ] << ")" << std::endl; - - // Get the static segment rotation in quaternion co-ordinates - Output_GetSegmentStaticRotationQuaternion _Output_GetSegmentStaticRotationQuaternion = - MyClient.GetSegmentStaticRotationQuaternion( SubjectName, SegmentName ); - output_stream << " Static Rotation Quaternion: (" << _Output_GetSegmentStaticRotationQuaternion.Rotation[ 0 ] << ", " - << _Output_GetSegmentStaticRotationQuaternion.Rotation[ 1 ] << ", " - << _Output_GetSegmentStaticRotationQuaternion.Rotation[ 2 ] << ", " - << _Output_GetSegmentStaticRotationQuaternion.Rotation[ 3 ] << ")" << std::endl; - - // Get the static segment rotation in EulerXYZ co-ordinates - Output_GetSegmentStaticRotationEulerXYZ _Output_GetSegmentStaticRotationEulerXYZ = - MyClient.GetSegmentStaticRotationEulerXYZ( SubjectName, SegmentName ); - output_stream << " Static Rotation EulerXYZ: (" << _Output_GetSegmentStaticRotationEulerXYZ.Rotation[ 0 ] << ", " - << _Output_GetSegmentStaticRotationEulerXYZ.Rotation[ 1 ] << ", " - << _Output_GetSegmentStaticRotationEulerXYZ.Rotation[ 2 ] << ")" << std::endl; - - // Get the global segment translation - Output_GetSegmentGlobalTranslation _Output_GetSegmentGlobalTranslation = - MyClient.GetSegmentGlobalTranslation( SubjectName, SegmentName ); - output_stream << " Global Translation: (" << _Output_GetSegmentGlobalTranslation.Translation[ 0 ] << ", " - << _Output_GetSegmentGlobalTranslation.Translation[ 1 ] << ", " - << _Output_GetSegmentGlobalTranslation.Translation[ 2 ] << ") " - << Adapt( _Output_GetSegmentGlobalTranslation.Occluded ) << std::endl; - - // Get the global segment rotation in helical co-ordinates - Output_GetSegmentGlobalRotationHelical _Output_GetSegmentGlobalRotationHelical = - MyClient.GetSegmentGlobalRotationHelical( SubjectName, SegmentName ); - output_stream << " Global Rotation Helical: (" << _Output_GetSegmentGlobalRotationHelical.Rotation[ 0 ] << ", " - << _Output_GetSegmentGlobalRotationHelical.Rotation[ 1 ] << ", " - << _Output_GetSegmentGlobalRotationHelical.Rotation[ 2 ] << ") " - << Adapt( _Output_GetSegmentGlobalRotationHelical.Occluded ) << std::endl; - - // Get the global segment rotation as a matrix - Output_GetSegmentGlobalRotationMatrix _Output_GetSegmentGlobalRotationMatrix = - MyClient.GetSegmentGlobalRotationMatrix( SubjectName, SegmentName ); - output_stream << " Global Rotation Matrix: (" << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 0 ] << ", " - << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 1 ] << ", " - << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 2 ] << ", " - << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 3 ] << ", " - << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 4 ] << ", " - << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 5 ] << ", " - << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 6 ] << ", " - << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 7 ] << ", " - << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 8 ] << ") " - << Adapt( _Output_GetSegmentGlobalRotationMatrix.Occluded ) << std::endl; - - // Get the global segment rotation in quaternion co-ordinates - Output_GetSegmentGlobalRotationQuaternion _Output_GetSegmentGlobalRotationQuaternion = - MyClient.GetSegmentGlobalRotationQuaternion( SubjectName, SegmentName ); - output_stream << " Global Rotation Quaternion: (" << _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 0 ] << ", " - << _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 1 ] << ", " - << _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 2 ] << ", " - << _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 3 ] << ") " - << Adapt( _Output_GetSegmentGlobalRotationQuaternion.Occluded ) << std::endl; - - // Get the global segment rotation in EulerXYZ co-ordinates - Output_GetSegmentGlobalRotationEulerXYZ _Output_GetSegmentGlobalRotationEulerXYZ = - MyClient.GetSegmentGlobalRotationEulerXYZ( SubjectName, SegmentName ); - output_stream << " Global Rotation EulerXYZ: (" << _Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 0 ] << ", " - << _Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 1 ] << ", " - << _Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 2 ] << ") " - << Adapt( _Output_GetSegmentGlobalRotationEulerXYZ.Occluded ) << std::endl; - - // Get the local segment translation - Output_GetSegmentLocalTranslation _Output_GetSegmentLocalTranslation = - MyClient.GetSegmentLocalTranslation( SubjectName, SegmentName ); - output_stream << " Local Translation: (" << _Output_GetSegmentLocalTranslation.Translation[ 0 ] << ", " - << _Output_GetSegmentLocalTranslation.Translation[ 1 ] << ", " - << _Output_GetSegmentLocalTranslation.Translation[ 2 ] << ") " - << Adapt( _Output_GetSegmentLocalTranslation.Occluded ) << std::endl; - - // Get the local segment rotation in helical co-ordinates - Output_GetSegmentLocalRotationHelical _Output_GetSegmentLocalRotationHelical = - MyClient.GetSegmentLocalRotationHelical( SubjectName, SegmentName ); - output_stream << " Local Rotation Helical: (" << _Output_GetSegmentLocalRotationHelical.Rotation[ 0 ] << ", " - << _Output_GetSegmentLocalRotationHelical.Rotation[ 1 ] << ", " - << _Output_GetSegmentLocalRotationHelical.Rotation[ 2 ] << ") " - << Adapt( _Output_GetSegmentLocalRotationHelical.Occluded ) << std::endl; - - // Get the local segment rotation as a matrix - Output_GetSegmentLocalRotationMatrix _Output_GetSegmentLocalRotationMatrix = - MyClient.GetSegmentLocalRotationMatrix( SubjectName, SegmentName ); - output_stream << " Local Rotation Matrix: (" << _Output_GetSegmentLocalRotationMatrix.Rotation[ 0 ] << ", " - << _Output_GetSegmentLocalRotationMatrix.Rotation[ 1 ] << ", " - << _Output_GetSegmentLocalRotationMatrix.Rotation[ 2 ] << ", " - << _Output_GetSegmentLocalRotationMatrix.Rotation[ 3 ] << ", " - << _Output_GetSegmentLocalRotationMatrix.Rotation[ 4 ] << ", " - << _Output_GetSegmentLocalRotationMatrix.Rotation[ 5 ] << ", " - << _Output_GetSegmentLocalRotationMatrix.Rotation[ 6 ] << ", " - << _Output_GetSegmentLocalRotationMatrix.Rotation[ 7 ] << ", " - << _Output_GetSegmentLocalRotationMatrix.Rotation[ 8 ] << ") " - << Adapt( _Output_GetSegmentLocalRotationMatrix.Occluded ) << std::endl; - - // Get the local segment rotation in quaternion co-ordinates - Output_GetSegmentLocalRotationQuaternion _Output_GetSegmentLocalRotationQuaternion = - MyClient.GetSegmentLocalRotationQuaternion( SubjectName, SegmentName ); - output_stream << " Local Rotation Quaternion: (" << _Output_GetSegmentLocalRotationQuaternion.Rotation[ 0 ] << ", " - << _Output_GetSegmentLocalRotationQuaternion.Rotation[ 1 ] << ", " - << _Output_GetSegmentLocalRotationQuaternion.Rotation[ 2 ] << ", " - << _Output_GetSegmentLocalRotationQuaternion.Rotation[ 3 ] << ") " - << Adapt( _Output_GetSegmentLocalRotationQuaternion.Occluded ) << std::endl; - - // Get the local segment rotation in EulerXYZ co-ordinates - Output_GetSegmentLocalRotationEulerXYZ _Output_GetSegmentLocalRotationEulerXYZ = - MyClient.GetSegmentLocalRotationEulerXYZ( SubjectName, SegmentName ); - output_stream << " Local Rotation EulerXYZ: (" << _Output_GetSegmentLocalRotationEulerXYZ.Rotation[ 0 ] << ", " - << _Output_GetSegmentLocalRotationEulerXYZ.Rotation[ 1 ] << ", " - << _Output_GetSegmentLocalRotationEulerXYZ.Rotation[ 2 ] << ") " - << Adapt( _Output_GetSegmentLocalRotationEulerXYZ.Occluded ) << std::endl; - } - - // Get the quality of the subject (object) if supported - Output_GetObjectQuality _Output_GetObjectQuality = MyClient.GetObjectQuality( SubjectName ); - if( _Output_GetObjectQuality.Result == Result::Success ) - { - double Quality = _Output_GetObjectQuality.Quality; - output_stream << " Quality: " << Quality << std::endl; - } - - // Count the number of markers - unsigned int MarkerCount = MyClient.GetMarkerCount( SubjectName ).MarkerCount; - output_stream << " Markers (" << MarkerCount << "):" << std::endl; - for( unsigned int MarkerIndex = 0 ; MarkerIndex < MarkerCount ; ++MarkerIndex ) - { - // Get the marker name - std::string MarkerName = MyClient.GetMarkerName( SubjectName, MarkerIndex ).MarkerName; - - // Get the marker parent - std::string MarkerParentName = MyClient.GetMarkerParentName( SubjectName, MarkerName ).SegmentName; - - // Get the global marker translation - Output_GetMarkerGlobalTranslation _Output_GetMarkerGlobalTranslation = - MyClient.GetMarkerGlobalTranslation( SubjectName, MarkerName ); - - output_stream << " Marker #" << MarkerIndex << ": " - << MarkerName << " (" - << _Output_GetMarkerGlobalTranslation.Translation[ 0 ] << ", " - << _Output_GetMarkerGlobalTranslation.Translation[ 1 ] << ", " - << _Output_GetMarkerGlobalTranslation.Translation[ 2 ] << ") " - << Adapt( _Output_GetMarkerGlobalTranslation.Occluded ) << std::endl; - - if( bReadRayData ) - { - Output_GetMarkerRayContributionCount _Output_GetMarkerRayContributionCount = - MyClient.GetMarkerRayContributionCount(SubjectName, MarkerName); - - if( _Output_GetMarkerRayContributionCount.Result == Result::Success ) - { - output_stream << " Contributed to by: "; - for( unsigned int ContributionIndex = 0; ContributionIndex < _Output_GetMarkerRayContributionCount.RayContributionsCount; ++ContributionIndex ) - { - Output_GetMarkerRayContribution _Output_GetMarkerRayContribution = - MyClient.GetMarkerRayContribution( SubjectName, MarkerName, ContributionIndex ); - output_stream << "ID:" << _Output_GetMarkerRayContribution.CameraID << " Index:" << _Output_GetMarkerRayContribution.CentroidIndex << " "; - } - output_stream << std::endl; - } - } - } - } - - // Get the unlabeled markers - unsigned int UnlabeledMarkerCount = MyClient.GetUnlabeledMarkerCount().MarkerCount; - output_stream << " Unlabeled Markers (" << UnlabeledMarkerCount << "):" << std::endl; - for( unsigned int UnlabeledMarkerIndex = 0 ; UnlabeledMarkerIndex < UnlabeledMarkerCount ; ++UnlabeledMarkerIndex ) - { - // Get the global marker translation - Output_GetUnlabeledMarkerGlobalTranslation _Output_GetUnlabeledMarkerGlobalTranslation = - MyClient.GetUnlabeledMarkerGlobalTranslation( UnlabeledMarkerIndex ); - - output_stream << " Marker #" << UnlabeledMarkerIndex << ": (" - << _Output_GetUnlabeledMarkerGlobalTranslation.Translation[ 0 ] << ", " - << _Output_GetUnlabeledMarkerGlobalTranslation.Translation[ 1 ] << ", " - << _Output_GetUnlabeledMarkerGlobalTranslation.Translation[ 2 ] << ")" << std::endl; - } - - // Get the labeled markers - unsigned int LabeledMarkerCount = MyClient.GetLabeledMarkerCount().MarkerCount; - output_stream << " Labeled Markers (" << LabeledMarkerCount << "):" << std::endl; - for (unsigned int LabeledMarkerIndex = 0; LabeledMarkerIndex < LabeledMarkerCount; ++LabeledMarkerIndex) - { - // Get the global marker translation - Output_GetLabeledMarkerGlobalTranslation _Output_GetLabeledMarkerGlobalTranslation = - MyClient.GetLabeledMarkerGlobalTranslation(LabeledMarkerIndex); - - output_stream << " Marker #" << LabeledMarkerIndex << ": (" - << _Output_GetLabeledMarkerGlobalTranslation.Translation[0] << ", " - << _Output_GetLabeledMarkerGlobalTranslation.Translation[1] << ", " - << _Output_GetLabeledMarkerGlobalTranslation.Translation[2] << ")" << std::endl; - } - - // Count the number of devices - unsigned int DeviceCount = MyClient.GetDeviceCount().DeviceCount; - output_stream << " Devices (" << DeviceCount << "):" << std::endl; - for( unsigned int DeviceIndex = 0 ; DeviceIndex < DeviceCount ; ++DeviceIndex ) - { - output_stream << " Device #" << DeviceIndex << ":" << std::endl; - - // Get the device name and type - Output_GetDeviceName _Output_GetDeviceName = MyClient.GetDeviceName( DeviceIndex ); - output_stream << " Name: " << _Output_GetDeviceName.DeviceName << std::endl; - output_stream << " Type: " << Adapt( _Output_GetDeviceName.DeviceType ) << std::endl; - - // Count the number of device outputs - unsigned int DeviceOutputCount = MyClient.GetDeviceOutputCount( _Output_GetDeviceName.DeviceName ).DeviceOutputCount; - output_stream << " Device Outputs (" << DeviceOutputCount << "):" << std::endl; - for( unsigned int DeviceOutputIndex = 0 ; DeviceOutputIndex < DeviceOutputCount ; ++DeviceOutputIndex ) - { - // Get the device output name and unit - Output_GetDeviceOutputName _Output_GetDeviceOutputName = - MyClient.GetDeviceOutputName( _Output_GetDeviceName.DeviceName, DeviceOutputIndex ); - - unsigned int DeviceOutputSubsamples = - MyClient.GetDeviceOutputSubsamples( _Output_GetDeviceName.DeviceName, - _Output_GetDeviceOutputName.DeviceOutputName ).DeviceOutputSubsamples; - - output_stream << " Device Output #" << DeviceOutputIndex << ":" << std::endl; - output_stream << " Samples (" << DeviceOutputSubsamples << "):" << std::endl; - - for( unsigned int DeviceOutputSubsample = 0; DeviceOutputSubsample < DeviceOutputSubsamples; ++DeviceOutputSubsample ) - { - output_stream << " Sample #" << DeviceOutputSubsample << ":" << std::endl; - - // Get the device output value - Output_GetDeviceOutputValue _Output_GetDeviceOutputValue = - MyClient.GetDeviceOutputValue( _Output_GetDeviceName.DeviceName, - _Output_GetDeviceOutputName.DeviceOutputName, - DeviceOutputSubsample ); - - output_stream << " '" << _Output_GetDeviceOutputName.DeviceOutputName << "' " - << _Output_GetDeviceOutputValue.Value << " " - << Adapt( _Output_GetDeviceOutputName.DeviceOutputUnit ) << " " - << Adapt( _Output_GetDeviceOutputValue.Occluded ) << std::endl; - } - } - } - - // Output the force plate information. - unsigned int ForcePlateCount = MyClient.GetForcePlateCount().ForcePlateCount; - output_stream << " Force Plates: (" << ForcePlateCount << ")" << std::endl; - - for( unsigned int ForcePlateIndex = 0 ; ForcePlateIndex < ForcePlateCount ; ++ForcePlateIndex ) - { - output_stream << " Force Plate #" << ForcePlateIndex << ":" << std::endl; - - unsigned int ForcePlateSubsamples = MyClient.GetForcePlateSubsamples( ForcePlateIndex ).ForcePlateSubsamples; - - output_stream << " Samples (" << ForcePlateSubsamples << "):" << std::endl; - - for( unsigned int ForcePlateSubsample = 0; ForcePlateSubsample < ForcePlateSubsamples; ++ForcePlateSubsample ) - { - output_stream << " Sample #" << ForcePlateSubsample << ":" << std::endl; - - Output_GetGlobalForceVector _Output_GetForceVector = MyClient.GetGlobalForceVector( ForcePlateIndex, ForcePlateSubsample ); - output_stream << " Force (" << _Output_GetForceVector.ForceVector[ 0 ] << ", "; - output_stream << _Output_GetForceVector.ForceVector[ 1 ] << ", "; - output_stream << _Output_GetForceVector.ForceVector[ 2 ] << ")" << std::endl; - - Output_GetGlobalMomentVector _Output_GetMomentVector = - MyClient.GetGlobalMomentVector( ForcePlateIndex, ForcePlateSubsample ); - output_stream << " Moment (" << _Output_GetMomentVector.MomentVector[ 0 ] << ", "; - output_stream << _Output_GetMomentVector.MomentVector[ 1 ] << ", "; - output_stream << _Output_GetMomentVector.MomentVector[ 2 ] << ")" << std::endl; - - Output_GetGlobalCentreOfPressure _Output_GetCentreOfPressure = - MyClient.GetGlobalCentreOfPressure( ForcePlateIndex, ForcePlateSubsample ); - output_stream << " CoP (" << _Output_GetCentreOfPressure.CentreOfPressure[ 0 ] << ", "; - output_stream << _Output_GetCentreOfPressure.CentreOfPressure[ 1 ] << ", "; - output_stream << _Output_GetCentreOfPressure.CentreOfPressure[ 2 ] << ")" << std::endl; - } - } - - // Output eye tracker information. - unsigned int EyeTrackerCount = MyClient.GetEyeTrackerCount().EyeTrackerCount; - output_stream << " Eye Trackers: (" << EyeTrackerCount << ")" << std::endl; - - for( unsigned int EyeTrackerIndex = 0 ; EyeTrackerIndex < EyeTrackerCount ; ++EyeTrackerIndex ) - { - output_stream << " Eye Tracker #" << EyeTrackerIndex << ":" << std::endl; - - Output_GetEyeTrackerGlobalPosition _Output_GetEyeTrackerGlobalPosition = MyClient.GetEyeTrackerGlobalPosition( EyeTrackerIndex ); - - output_stream << " Position (" << _Output_GetEyeTrackerGlobalPosition.Position[ 0 ] << ", "; - output_stream << _Output_GetEyeTrackerGlobalPosition.Position[ 1 ] << ", "; - output_stream << _Output_GetEyeTrackerGlobalPosition.Position[ 2 ] << ") "; - output_stream << Adapt( _Output_GetEyeTrackerGlobalPosition.Occluded ) << std::endl; - - Output_GetEyeTrackerGlobalGazeVector _Output_GetEyeTrackerGlobalGazeVector = MyClient.GetEyeTrackerGlobalGazeVector( EyeTrackerIndex ); - - output_stream << " Gaze (" << _Output_GetEyeTrackerGlobalGazeVector.GazeVector[ 0 ] << ", "; - output_stream << _Output_GetEyeTrackerGlobalGazeVector.GazeVector[ 1 ] << ", "; - output_stream << _Output_GetEyeTrackerGlobalGazeVector.GazeVector[ 2 ] << ") "; - output_stream << Adapt( _Output_GetEyeTrackerGlobalGazeVector.Occluded ) << std::endl; - } - - if( bReadCentroids ) - { - unsigned int CameraCount = MyClient.GetCameraCount().CameraCount; - output_stream << "Cameras(" << CameraCount << "):" << std::endl; - - for( unsigned int CameraIndex = 0; CameraIndex < CameraCount; ++CameraIndex ) - { - output_stream << " Camera #" << CameraIndex << ":" << std::endl; - - const std::string CameraName = MyClient.GetCameraName( CameraIndex ).CameraName; - output_stream << " Name: " << CameraName << std::endl; - - output_stream << " Id: " << MyClient.GetCameraId(CameraName).CameraId << std::endl; - output_stream << " User Id: " << MyClient.GetCameraUserId(CameraName).CameraUserId << std::endl; - output_stream << " Type: " << MyClient.GetCameraType(CameraName).CameraType << std::endl; - output_stream << " Display Name: " << MyClient.GetCameraDisplayName(CameraName).CameraDisplayName << std::endl; - Output_GetCameraResolution _Output_GetCameraResolution = MyClient.GetCameraResolution(CameraName); - output_stream << " Resolution: " << _Output_GetCameraResolution.ResolutionX << " x " << _Output_GetCameraResolution.ResolutionY << std::endl; - output_stream << " Is Video Camera: " << ( MyClient.GetIsVideoCamera(CameraName).IsVideoCamera ? "true":"false" ) << std::endl; - - unsigned int CentroidCount = MyClient.GetCentroidCount( CameraName ).CentroidCount; - output_stream << " Centroids(" << CentroidCount << "):" << std::endl; - - for( unsigned int CentroidIndex = 0; CentroidIndex < CentroidCount; ++CentroidIndex ) - { - output_stream << " Centroid #" << CentroidIndex << ":" << std::endl; - - Output_GetCentroidPosition _Output_GetCentroidPosition = MyClient.GetCentroidPosition( CameraName, CentroidIndex ); - output_stream << " Position: (" << _Output_GetCentroidPosition.CentroidPosition[0] << ", " - << _Output_GetCentroidPosition.CentroidPosition[1] << ")" << std::endl; - output_stream << " Radius: (" << _Output_GetCentroidPosition.Radius << ")" << std::endl; - //output_stream << " Accuracy: (" << _Output_GetCentroidPosition.Accuracy << ")" << std::endl; - - Output_GetCentroidWeight _Output_GetCentroidWeight = MyClient.GetCentroidWeight( CameraName, CentroidIndex ); - if( _Output_GetCentroidWeight.Result == Result::Success ) - { - output_stream << " Weighting: " << _Output_GetCentroidWeight.Weight << std::endl; - } - } - - if (bReadGreyscaleData) - { - unsigned int BlobCount = MyClient.GetGreyscaleBlobCount(CameraName).BlobCount; - output_stream << " Blobs(" << BlobCount << "):" << std::endl; - for (unsigned int BlobIndex = 0; BlobIndex < BlobCount; ++BlobIndex) - { - output_stream << " Blob #" << BlobIndex << ":" << std::endl; - - Output_GetGreyscaleBlob _Output_GetGreyscaleBlob = MyClient.GetGreyscaleBlob(CameraName, BlobIndex); - if (_Output_GetGreyscaleBlob.Result == Result::Success) - { - // Don't print out the whole blob - output_stream << " # Line Positions X: " << _Output_GetGreyscaleBlob.BlobLinePositionsX.size() << std::endl; - output_stream << " # Line Positions Y: " << _Output_GetGreyscaleBlob.BlobLinePositionsY.size() << std::endl; - output_stream << " # Pixel Values: " << _Output_GetGreyscaleBlob.BlobLinePixelValues.size() << std::endl; - } - } - } - - if (bReadVideoData) - { - Output_GetVideoFrame _Output_GetVideoFrame = MyClient.GetVideoFrame(CameraName); - if (_Output_GetVideoFrame.Result == Result::Success) - { - output_stream << " Video Frame:" << std::endl; - output_stream << " Width:" << _Output_GetVideoFrame.m_Width << std::endl; - output_stream << " Height:" << _Output_GetVideoFrame.m_Height << std::endl; - output_stream << " Frame Id:" << _Output_GetVideoFrame.m_FrameID << std::endl; - output_stream << " Format:" << _Output_GetVideoFrame.m_Format << std::endl; - if (_Output_GetVideoFrame.m_Data) - { - output_stream << " Data:" << _Output_GetVideoFrame.m_Data->size() << " samples" << std::endl; - } - } - } - } - } - } - - if( EnableMultiCast ) - { - MyClient.StopTransmittingMulticast(); - } - MyClient.DisableSegmentData(); - MyClient.DisableMarkerData(); - MyClient.DisableUnlabeledMarkerData(); - MyClient.DisableDeviceData(); - if( bReadCentroids ) - { - MyClient.DisableCentroidData(); - } - if( bReadRayData ) - { - MyClient.DisableMarkerRayData(); - } - if (bReadGreyscaleData) - { - MyClient.DisableGreyscaleData(); - } - if (bReadVideoData) - { - MyClient.DisableVideoData(); - } - - // Disconnect and dispose - int t = clock(); - std::cout << " Disconnecting..." << std::endl; - MyClient.Disconnect(); - int dt = clock() - t; - double secs = (double) (dt)/(double)CLOCKS_PER_SEC; - std::cout << " Disconnect time = " << secs << " secs" << std::endl; - - } -} diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPPTest/makefile b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPPTest/makefile deleted file mode 100644 index 902bb63aa..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPPTest/makefile +++ /dev/null @@ -1,107 +0,0 @@ -# Makefile for ViconDataStreamSDK_CPPTest - -ifndef VERBOSE -.SILENT : -endif -.SUFFIXES : - -ifdef CONFIG -ifneq ($(CONFIG),Debug) -ifneq ($(CONFIG),InternalRelease) -ifneq ($(CONFIG),Release) -Error: unknown configuration. -endif -endif -endif -else -CONFIG=Debug -endif - -INCLUDEPATHS=../ViconDataStreamSDK_CPP ../../../../thirdparty/Boost/boost-1.58.0-dynamic-linux-x64/installed/include . ../.. .. -INCLUDEPATHS_Debug=Debug -INCLUDEPATHS_InternalRelease=InternalRelease -INCLUDEPATHS_Release=Release -LIBRARYPATHS=../../../../thirdparty/Boost/boost-1.58.0-dynamic-linux-x64/installed/lib -LIBRARYPATHS_Debug=../../../../lib/Debug ../../../../bin/Debug -LIBRARYPATHS_InternalRelease=../../../../lib/InternalRelease ../../../../bin/InternalRelease -LIBRARYPATHS_Release=../../../../lib/Release ../../../../bin/Release -DEPENDENCIES=ViconDataStreamSDKCore ViconDataStreamSDK_CPP ViconCGStreamClientSDK ViconCGStreamClient ViconCGStream StreamCommon -LIBRARIES= -LIBRARIES_Debug=boost_wserialization-mt-d boost_wave-mt-d boost_unit_test_framework-mt-d boost_timer-mt-d boost_thread-mt-d boost_system-mt-d boost_signals-mt-d boost_serialization-mt-d boost_regex-mt-d boost_random-mt-d boost_python-mt-d boost_program_options-mt-d boost_prg_exec_monitor-mt-d boost_math_tr1l-mt-d boost_math_tr1f-mt-d boost_math_tr1-mt-d boost_math_c99l-mt-d boost_math_c99f-mt-d boost_math_c99-mt-d boost_log_setup-mt-d boost_log-mt-d boost_locale-mt-d boost_iostreams-mt-d boost_graph-mt-d boost_filesystem-mt-d boost_date_time-mt-d boost_coroutine-mt-d boost_context-mt-d boost_container-mt-d boost_chrono-mt-d boost_atomic-mt-d -LIBRARIES_InternalRelease=boost_wserialization-mt boost_wave-mt boost_unit_test_framework-mt boost_timer-mt boost_thread-mt boost_system-mt boost_signals-mt boost_serialization-mt boost_regex-mt boost_random-mt boost_python-mt boost_program_options-mt boost_prg_exec_monitor-mt boost_math_tr1l-mt boost_math_tr1f-mt boost_math_tr1-mt boost_math_c99l-mt boost_math_c99f-mt boost_math_c99-mt boost_log_setup-mt boost_log-mt boost_locale-mt boost_iostreams-mt boost_graph-mt boost_filesystem-mt boost_date_time-mt boost_coroutine-mt boost_context-mt boost_container-mt boost_chrono-mt boost_atomic-mt -LIBRARIES_Release=boost_wserialization-mt boost_wave-mt boost_unit_test_framework-mt boost_timer-mt boost_thread-mt boost_system-mt boost_signals-mt boost_serialization-mt boost_regex-mt boost_random-mt boost_python-mt boost_program_options-mt boost_prg_exec_monitor-mt boost_math_tr1l-mt boost_math_tr1f-mt boost_math_tr1-mt boost_math_c99l-mt boost_math_c99f-mt boost_math_c99-mt boost_log_setup-mt boost_log-mt boost_locale-mt boost_iostreams-mt boost_graph-mt boost_filesystem-mt boost_date_time-mt boost_coroutine-mt boost_context-mt boost_container-mt boost_chrono-mt boost_atomic-mt -DEFINES=TCM_LINUX TCM_UNIX PROJECT_SOURCE_PATH=\"/home/swtest/workspace/DatastreamSDK_Linux_64_Branch_Release/Binary/bin/Release/deployment/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPPTest\" -DEFINES_Debug=_DEBUG -DEFINES_InternalRelease=VICON_INTERNAL_RELEASE NDEBUG -DEFINES_Release=NDEBUG TCM_OFF_SITE -ENV_CPU=x64 - -SOURCEDIRECTORY=../../../.. -PROJECTPATH=. -BINARYDIRECTORY=../../../.. -INTERMEDIATEDIRECTORY=. -LIBRARYDIRECTORY=../../../../lib -OUTPUTDIRECTORY=../../../../bin - -include $(BINARYDIRECTORY)/gcc.mk - -HIDE_BOOST_SCRIPT=hide_boost_version_script -ifneq ($(HIDE_BOOST),) - HIDE_BOOST_LD_PARAM= -Wl,--version-script=$(HIDE_BOOST_SCRIPT) - HIDE_BOOST_LD_PREREQ=$(HIDE_BOOST_SCRIPT) -endif -all: all_$(CONFIG) - -all_Debug: $(OUTPUTDIRECTORY)/$(CONFIG)/ViconDataStreamSDK_CPPTest -all_InternalRelease: $(OUTPUTDIRECTORY)/$(CONFIG)/ViconDataStreamSDK_CPPTest -all_Release: $(OUTPUTDIRECTORY)/$(CONFIG)/ViconDataStreamSDK_CPPTest - -OBJECTS=$(CONFIG)/ViconDataStreamSDK_CPPTest.o - -CXXFLAGS=$(CXXFLAGS_$(CONFIG)) $(L_INCLUDEPATHS) $(L_INCLUDEPATHS_$(CONFIG)) $(L_INCLUDES) $(L_INCLUDES_$(CONFIG)) $(L_DEFINES) $(L_DEFINES_$(CONFIG)) -CCFLAGS=$(CCFLAGS_$(CONFIG)) $(L_INCLUDEPATHS) $(L_INCLUDEPATHS_$(CONFIG)) $(L_INCLUDES) $(L_INCLUDES_$(CONFIG)) $(L_DEFINES) $(L_DEFINES_$(CONFIG)) -LDFLAGS=$(LDFLAGS_$(CONFIG)) $(L_LIBRARYPATHS) $(L_LIBRARYPATHS_$(CONFIG)) -CXXFLAGS+=-Wno-unused-local-typedefs -CXXFLAGS+=-Wno-delete-non-virtual-dtor -std=c++14 -#Android toolchain does not include librt but integrates some of its functionality into Android libc. -ifndef ANDROID_TARGET_ARCH -LDFLAGS+=-lrt -endif - - -$(OUTPUTDIRECTORY)/Debug/ViconDataStreamSDK_CPPTest: makefile $(OBJECTS) $(LIBRARYDIRECTORY)/$(CONFIG)/libStreamCommon.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStream.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClient.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClientSDK.a $(OUTPUTDIRECTORY)/$(CONFIG)/libViconDataStreamSDK_CPP.so $(LIBRARYDIRECTORY)/$(CONFIG)/libViconDataStreamSDKCore.a - @echo \[1\;32mLinking EXE $@\ - @mkdir -p $(@D) - $(LD) -Wl,--as-needed -export-dynamic $(LDFLAGS) -o $@ $(OBJECTS) -Wl,--start-group $(L_DEPENDENCIES) $(L_LIBRARIES_$(CONFIG)) $(L_LIBRARIES) -Wl,--end-group -pthread -ldl -Wl,-rpath='$$ORIGIN' -Wl,-rpath-link=.:$(@D) - -$(OUTPUTDIRECTORY)/InternalRelease/ViconDataStreamSDK_CPPTest: makefile $(OBJECTS) $(LIBRARYDIRECTORY)/$(CONFIG)/libStreamCommon.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStream.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClient.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClientSDK.a $(OUTPUTDIRECTORY)/$(CONFIG)/libViconDataStreamSDK_CPP.so $(LIBRARYDIRECTORY)/$(CONFIG)/libViconDataStreamSDKCore.a - @echo \[1\;32mLinking EXE $@\ - @mkdir -p $(@D) - $(LD) -Wl,--as-needed -export-dynamic $(LDFLAGS) -o $@ $(OBJECTS) -Wl,--start-group $(L_DEPENDENCIES) $(L_LIBRARIES_$(CONFIG)) $(L_LIBRARIES) -Wl,--end-group -pthread -ldl -Wl,-rpath='$$ORIGIN' -Wl,-rpath-link=.:$(@D) - -$(OUTPUTDIRECTORY)/Release/ViconDataStreamSDK_CPPTest: makefile $(OBJECTS) $(LIBRARYDIRECTORY)/$(CONFIG)/libStreamCommon.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStream.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClient.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClientSDK.a $(OUTPUTDIRECTORY)/$(CONFIG)/libViconDataStreamSDK_CPP.so $(LIBRARYDIRECTORY)/$(CONFIG)/libViconDataStreamSDKCore.a - @echo \[1\;32mLinking EXE $@\ - @mkdir -p $(@D) - $(LD) -Wl,--as-needed -export-dynamic $(LDFLAGS) -o $@ $(OBJECTS) -Wl,--start-group $(L_DEPENDENCIES) $(L_LIBRARIES_$(CONFIG)) $(L_LIBRARIES) -Wl,--end-group -pthread -ldl -Wl,-rpath='$$ORIGIN' -Wl,-rpath-link=.:$(@D) - -# Source Files -$(CONFIG)/ViconDataStreamSDK_CPPTest.o: makefile $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPPTest/ViconDataStreamSDK_CPPTest.cpp - @echo \[1\;34mCompiling ViconDataStreamSDK_CPPTest.cpp\ - @mkdir -p $(@D) - find $(CONFIG) -name *.gch -exec cp '{}' . \; - $(CXX) -fPIC -MMD -MP -I$(CONFIG)/ $(CXXFLAGS) -o $@ -c $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CPPTest/ViconDataStreamSDK_CPPTest.cpp - --include $(CONFIG)/ViconDataStreamSDK_CPPTest.d - -# Other Files - -clean: - @echo \[1\;31mCleaning $(CONFIG) build\ - find . -path '*/$(CONFIG)/*' \( -name '*.[od]' -o -name '*.gch' \) -exec rm -f {} ';' - rm -f moc_*.cxx - -$(HIDE_BOOST_SCRIPT): makefile - echo -n >$@ - echo "{" >>$@ - echo " local: *N5boost*; *NK5boost*;" >>$@ - echo "};" >>$@ diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CTest/ViconDataStreamSDK_CTest.c b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CTest/ViconDataStreamSDK_CTest.c deleted file mode 100644 index 872bd7564..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CTest/ViconDataStreamSDK_CTest.c +++ /dev/null @@ -1,1062 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// - -#include -#include - -#pragma warning( push ) -#pragma warning( disable:4255 ) - -#include -#include -#include - -#ifdef _WIN32 -#include -#include -#else -#include -#endif - -#include - -#pragma warning( pop ) - - - CString Adapt(const CBool i_Value) - { - return i_Value ? "True" : "False"; - } - - CString AdaptStandard( CTimecodeStandard i_Standard ) - { - switch( i_Standard ) - { - default: - case None: - return "0"; - case PAL: - return "1"; - case NTSC: - return "2"; - case NTSCDrop: - return "3"; - case Film: - return "4"; - case NTSCFilm: - return "5"; - case ATSC: - return "6"; - } - } - - CString AdaptDirection( CDirection i_Direction) - { - switch (i_Direction) - { - case Forward: - return "Forward"; - case Backward: - return "Backward"; - case Left: - return "Left"; - case Right: - return "Right"; - case Up: - return "Up"; - case Down: - return "Down"; - default: - return "Unknown"; - } - } - - CString AdaptDeviceType( CDeviceType i_DeviceType) - { - switch (i_DeviceType) - { - case ForcePlate: - return "ForcePlate"; - default: - return "Unknown"; - } - } - - CString AdaptUnit( CUnit i_Unit) - { - switch (i_Unit) - { - case Meter: - return "Meter"; - case Volt: - return "Volt"; - case NewtonMeter: - return "NewtonMeter"; - case Newton: - return "Newton"; - case Kilogram: - return "Kilogram"; - case Second: - return "Second"; - case Ampere: - return "Ampere"; - case Kelvin: - return "Kelvin"; - case Mole: - return "Mole"; - case Candela: - return "Candela"; - case Radian: - return "Radian"; - case Steradian: - return "Steradian"; - case MeterSquared: - return "MeterSquared"; - case MeterCubed: - return "MeterCubed"; - case MeterPerSecond: - return "MeterPerSecond"; - case MeterPerSecondSquared: - return "MeterPerSecondSquared"; - case RadianPerSecond: - return "RadianPerSecond"; - case RadianPerSecondSquared: - return "RadianPerSecondSquared"; - case Hertz: - return "Hertz"; - case Joule: - return "Joule"; - case Watt: - return "Watt"; - case Pascal: - return "Pascal"; - case Lumen: - return "Lumen"; - case Lux: - return "Lux"; - case Coulomb: - return "Coulomb"; - case Ohm: - return "Ohm"; - case Farad: - return "Farad"; - case Weber: - return "Weber"; - case Tesla: - return "Tesla"; - case Henry: - return "Henry"; - case Siemens: - return "Siemens"; - case Becquerel: - return "Becquerel"; - case Gray: - return "Gray"; - case Sievert: - return "Sievert"; - case Katal: - return "Katal"; - default: - return "Unknown"; - } - } - -#ifdef WIN32 - CBool Hit(void) - { - CBool hit = 0; - while (_kbhit()) - { - getchar(); - hit = 1; - } - return hit; - } -#endif - -int main(int argc, char* argv[]) -{ - // Program options - - CString HostName = "localhost:801"; - if (argc > 1) - { - HostName = argv[1]; - } - - // log contains: - // version number - // log of framerate over time - // --multicast - // kill off internal app - CString LogFile = ""; - CString MulticastAddress = "244.0.0.0:44801"; - CBool ConnectToMultiCast = 0; - CBool EnableMultiCast = 0; - CBool bReadCentroids = 0; - CBool bReadRayData = 0; - - unsigned int ClientBufferSize = 0; - CString AxisMapping = "ZUp"; - - int a; - for (a = 2; a < argc; ++a) - { - CString arg = argv[a]; - if ( strcmp( arg, "--help") == 0 ) - { - printf( argv[0], " : allowed options include:\n --log_file --enable_multicast --connect_to_multicast --help --centroids --client-buffer-size \n"); - return 0; - } - else if ( strcmp( arg, "--log_file") == 0 ) - { - if (a < argc) - { - LogFile = argv[a + 1]; - printf("Using log file < %s> ...\n", LogFile); - ++a; - } - } - else if ( strcmp( arg, "--enable_multicast") == 0 ) - { - EnableMultiCast = 1; - if (a < argc) - { - CString MulticastAddress = argv[a + 1]; - printf( "Enabling multicast address <%s> ...\n", MulticastAddress); - a = a +1; - } - } - else if ( strcmp( arg, "--connect_to_multicast") == 0 ) - { - ConnectToMultiCast = 1; - if (a < argc) - { - MulticastAddress = argv[a + 1]; - printf( "connecting to multicast address <%s> ...\n", MulticastAddress ); - ++a; - } - } - else if ( strcmp( arg, "--centroids") == 0 ) - { - bReadCentroids = 1; - } - else if( strcmp( arg, "--rays" ) == 0 ) - { - bReadRayData = 1; - } - else if (strcmp(arg, "--greyscale") == 0) - { - printf("Greyscale data not supported by the C Client\n"); - } - else if (strcmp(arg, "--video") == 0) - { - printf("Video data not supported by the C Client\n"); - } - else if( strcmp( arg, "--client-buffer-size" ) == 0 ) - { - ++a; - if (a < argc) - { - ClientBufferSize = atoi(argv[a]); - } - } - else if( strcmp( arg, "--set-axis-mapping" ) == 0 ) - { - ++a; - if (a < argc) - { - AxisMapping = argv[a]; - - if ( strcmp( AxisMapping, "XUp" ) == 0 || strcmp( AxisMapping, "YUp" ) == 0 || strcmp( AxisMapping, "ZUp") == 0 ) - { - printf( "Setting Axis to %s\n", AxisMapping); - } - else - { - printf( "Unknown axis setting: %s . Should be XUp, YUp, or ZUp\n", AxisMapping); - return 1; - } - } - } - else - { - printf("Failed to understand argument <%s>...exiting\n", argv[a]); - return 1; - } - } - - /* - std::ofstream ofs; - if (!LogFile.empty()) - { - ofs.open(LogFile.c_str()); - if (!ofs.is_open()) - { - printf( "Could not open log file <", LogFile, ">...exiting", "\n"); - return 1; - } - } - */ - - // Make a new client - CClient* pClient = Client_Create(); - - int i; - for (i = 0; i != 3; ++i) // repeat to check disconnecting doesn't wreck next connect - { - // Connect to a server - printf( "Connecting to %s...", HostName); - while (!Client_IsConnected(pClient)) - { - // Direct connection - - CBool ok = 0; - if (ConnectToMultiCast) - { - // Multicast connection - ok = (Client_ConnectToMulticast(pClient, HostName, MulticastAddress)); - - } - else - { - ok = Client_Connect(pClient, HostName); - } - if (!ok) - { - printf("Warning - connect failed...\n"); - } - - - printf("."); -#ifdef WIN32 - Sleep(1000); -#else - sleep(1); -#endif - } - printf("\n"); - - - // Enable some different data types - Client_EnableSegmentData(pClient); - Client_EnableMarkerData(pClient); - Client_EnableUnlabeledMarkerData(pClient); - Client_EnableMarkerRayData(pClient); - Client_EnableDeviceData(pClient); - Client_EnableDebugData(pClient); - if (bReadCentroids) - { - Client_EnableCentroidData(pClient); - } - if (bReadRayData) - { - Client_EnableMarkerRayData(pClient); - } - - printf( "Segment Data Enabled: %s\n", Adapt(Client_IsSegmentDataEnabled(pClient))); - printf( "Marker Data Enabled: %s\n", Adapt(Client_IsMarkerDataEnabled(pClient))); - printf( "Unlabeled Marker Data Enabled: %s\n", Adapt(Client_IsUnlabeledMarkerDataEnabled(pClient))); - printf( "Device Data Enabled: %s\n", Adapt(Client_IsDeviceDataEnabled(pClient))); - printf( "Centroid Data Enabled: %s\n", Adapt(Client_IsCentroidDataEnabled(pClient))); - printf( "Marker Ray Data Enabled: %s\n", Adapt(Client_IsMarkerRayDataEnabled(pClient))); - - // Set the streaming mode - //Client_SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ClientPull ); - // Client_SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ClientPullPreFetch ); - Client_SetStreamMode(pClient, ServerPush); - - // Set the global up axis - Client_SetAxisMapping(pClient, Forward, Left, Up); // Z-up - - if (strcmp( AxisMapping, "YUp") == 0 ) - { - Client_SetAxisMapping(pClient, Forward, Up, Right); // Y-up - } - else if( strcmp(AxisMapping, "XUp") == 0 ) - { - Client_SetAxisMapping(pClient, Up, Forward, Left); // Y-up - } - - COutput_GetAxisMapping _Output_GetAxisMapping; - Client_GetAxisMapping(pClient, &_Output_GetAxisMapping); - printf( "Axis Mapping: X-%s Y-%s Z-%s\n" - , AdaptDirection(_Output_GetAxisMapping.XAxis) - , AdaptDirection(_Output_GetAxisMapping.YAxis) - , AdaptDirection(_Output_GetAxisMapping.ZAxis) - ); - - // Discover the version number - COutput_GetVersion _Output_GetVersion; - Client_GetVersion(pClient, &_Output_GetVersion); - printf( "Version: %d.%d.%d\n", _Output_GetVersion.Major, _Output_GetVersion.Minor , _Output_GetVersion.Point); - - if (EnableMultiCast) - { - - //assert(Adapt(Client_IsConnected(pClient))); - Client_StartTransmittingMulticast(pClient, HostName, MulticastAddress); - } - - unsigned int FrameRateWindow = 1000; // frames - unsigned int Counter = 0; - clock_t LastTime = clock(); - // Loop until a key is pressed -#ifdef WIN32 - while (!Hit()) -#else - while (1) -#endif - { - // Get a frame - printf( "Waiting for new frame..." ); - while (Client_GetFrame(pClient) != Success) - { - // Sleep a little so that we don't lumber the CPU with a busy poll -#ifdef WIN32 - Sleep(200); -#else - sleep(1); -#endif - - printf( "." ); - } - printf( "\n" ); - if (++Counter == FrameRateWindow) - { - clock_t Now = clock(); - double FrameRate = (double)(FrameRateWindow * CLOCKS_PER_SEC) / (double)(Now - LastTime); - if (!LogFile) - { - time_t rawtime; - struct tm * timeinfo = 0; - time(&rawtime); -#ifdef _WIN32 - localtime_s(timeinfo, &rawtime); - size_t TimeBufSize = 128; - char TimeBuf[ 128 ]; - asctime_s( TimeBuf, TimeBufSize, timeinfo ); - printf( "Frame rate = %f at %s\n", FrameRate, TimeBuf ); -#else - timeinfo = localtime(&rawtime); - char * TimeBuf = asctime( timeinfo ); - printf( "Frame rate = %f at %s\n", FrameRate, TimeBuf ); -#endif - } - - LastTime = Now; - Counter = 0; - } - - // Get the frame number - COutput_GetFrameNumber _Output_GetFrameNumber; - Client_GetFrameNumber(pClient, &_Output_GetFrameNumber); - printf( "Frame Number: %d\n", _Output_GetFrameNumber.FrameNumber); - - COutput_GetFrameRate Rate; - Client_GetFrameRate(pClient, &Rate); - // Using %g rather than %f in an attempt to copy std::cout's default output formatting as used in the other clients. - // From https://stackoverflow.com/questions/2411903/getting-printf-to-drop-the-trailing-0-of-values - printf( "Frame rate: %.6g\n", Rate.FrameRateHz ); - - // Show frame rates - COutput_GetFrameRateCount RateCount; - Client_GetFrameRateCount(pClient, &RateCount); - - unsigned int FramerateIndex; - for (FramerateIndex = 0; FramerateIndex < RateCount.Count; ++FramerateIndex) - { - char FramerateName[128]; - Client_GetFrameRateName(pClient, FramerateIndex, 128, FramerateName); - COutput_GetFrameRateValue FramerateValue; - Client_GetFrameRateValue(pClient, FramerateName, &FramerateValue); - - printf( "%s: %.6gHz\n", FramerateName, FramerateValue.Value ); - } - printf( "\n" ); - - // Get the timecode - COutput_GetTimecode _Output_GetTimecode; - Client_GetTimecode(pClient, &_Output_GetTimecode); - - //TODO Standard enum may cause an issue but this may be a bug in the cpp code! - printf( "Timecode: %dh %dm %ds %df %dsf %s %s %d %d\n\n" - , _Output_GetTimecode.Hours - , _Output_GetTimecode.Minutes - , _Output_GetTimecode.Seconds - , _Output_GetTimecode.Frames - , _Output_GetTimecode.SubFrame - , Adapt(_Output_GetTimecode.FieldFlag) - , AdaptStandard(_Output_GetTimecode.Standard) - , _Output_GetTimecode.SubFramesPerFrame - , _Output_GetTimecode.UserBits - ); - - // Get the latency - - COutput_GetLatencyTotal LatencyTotal; - Client_GetLatencyTotal(pClient, &LatencyTotal); - printf( "Latency: %fs\n", LatencyTotal.Total); - - COutput_GetLatencySampleCount LatencySampleCount; - Client_GetLatencySampleCount(pClient, &LatencySampleCount); - unsigned int LatencySampleIndex; - for (LatencySampleIndex = 0; LatencySampleIndex < LatencySampleCount.Count; ++LatencySampleIndex) - { - char SampleName[128]; - Client_GetLatencySampleName(pClient, LatencySampleIndex, 128, SampleName); - COutput_GetLatencySampleValue SampleValue; - Client_GetLatencySampleValue(pClient, SampleName, &SampleValue); - - printf( " %s %fs\n", SampleName, SampleValue.Value ); - } - printf( "\n" ); - - COutput_GetHardwareFrameNumber _Output_GetHardwareFrameNumber; - Client_GetHardwareFrameNumber(pClient, &_Output_GetHardwareFrameNumber); - printf( "Hardware Frame Number: %d\n", _Output_GetHardwareFrameNumber.HardwareFrameNumber ); - - - - // Count the number of subjects - COutput_GetSubjectCount SubjectCount; - Client_GetSubjectCount(pClient, &SubjectCount); - printf( "Subjects (%d):\n", SubjectCount.SubjectCount ); - unsigned int SubjectIndex; - for (SubjectIndex = 0; SubjectIndex < SubjectCount.SubjectCount; ++SubjectIndex) - { - printf( " Subject #%d\n", SubjectIndex); - - // Get the subject name - char SubjectName[128]; - Client_GetSubjectName(pClient, SubjectIndex, 128, SubjectName); - printf( " Name: %s\n", SubjectName); - - // Get the root segment - char RootSegment[128]; - Client_GetSubjectRootSegmentName(pClient, SubjectName, 128, RootSegment); - printf( " Root Segment: %s\n", RootSegment); - - // Count the number of segments - COutput_GetSegmentCount SegmentCount; - Client_GetSegmentCount(pClient, SubjectName, &SegmentCount); - - printf( " Segments (%d):\n", SegmentCount.SegmentCount); - unsigned int SegmentIndex; - for (SegmentIndex = 0; SegmentIndex < SegmentCount.SegmentCount; ++SegmentIndex) - { - printf( " Segment #%d\n", SegmentIndex); - - // Get the segment name - char SegmentName[128]; - Client_GetSegmentName(pClient, SubjectName, SegmentIndex, 128, SegmentName); - printf( " Name: %s\n", SegmentName); - - // Get the segment parent - char SegmentParentName[128]; - Client_GetSegmentParentName(pClient, SubjectName, SegmentName, 128, SegmentParentName); - printf( " Parent: %s\n", SegmentParentName ); - - // Get the segment's children - COutput_GetSegmentChildCount ChildCount; - Client_GetSegmentChildCount(pClient, SubjectName, SegmentName, &ChildCount); - printf( " Children (%d):\n", ChildCount.SegmentCount ); - unsigned int ChildIndex; - for (ChildIndex = 0; ChildIndex < ChildCount.SegmentCount; ++ChildIndex) - { - char SegmentChildName[128]; - Client_GetSegmentChildName( pClient, SubjectName, SegmentName, ChildIndex, 128, SegmentChildName ); - printf( " %s\n", SegmentChildName ); - } - - // Get the static segment translation - COutput_GetSegmentStaticTranslation _Output_GetSegmentStaticTranslation; - Client_GetSegmentStaticTranslation(pClient, SubjectName, SegmentName, &_Output_GetSegmentStaticTranslation); - printf( " Static Translation: (%f, %f, %f)\n" - , _Output_GetSegmentStaticTranslation.Translation[0] - , _Output_GetSegmentStaticTranslation.Translation[1] - , _Output_GetSegmentStaticTranslation.Translation[2] - ); - - // Get the static segment rotation in helical co-ordinates - COutput_GetSegmentStaticRotationHelical _Output_GetSegmentStaticRotationHelical; - Client_GetSegmentStaticRotationHelical(pClient, SubjectName, SegmentName, &_Output_GetSegmentStaticRotationHelical); - printf( " Static Rotation Helical: (%f, %f, %f)\n" - , _Output_GetSegmentStaticRotationHelical.Rotation[0] - , _Output_GetSegmentStaticRotationHelical.Rotation[1] - , _Output_GetSegmentStaticRotationHelical.Rotation[2] - ); - - // Get the static segment rotation as a matrix - COutput_GetSegmentStaticRotationMatrix _Output_GetSegmentStaticRotationMatrix; - Client_GetSegmentStaticRotationMatrix(pClient, SubjectName, SegmentName, &_Output_GetSegmentStaticRotationMatrix); - printf( " Static Rotation Matrix: (%f, %f, %f, %f, %f, %f, %f, %f, %f)\n" - , _Output_GetSegmentStaticRotationMatrix.Rotation[0] - , _Output_GetSegmentStaticRotationMatrix.Rotation[1] - , _Output_GetSegmentStaticRotationMatrix.Rotation[2] - , _Output_GetSegmentStaticRotationMatrix.Rotation[3] - , _Output_GetSegmentStaticRotationMatrix.Rotation[4] - , _Output_GetSegmentStaticRotationMatrix.Rotation[5] - , _Output_GetSegmentStaticRotationMatrix.Rotation[6] - , _Output_GetSegmentStaticRotationMatrix.Rotation[7] - , _Output_GetSegmentStaticRotationMatrix.Rotation[8] - ); - - // Get the static segment rotation in quaternion co-ordinates - COutput_GetSegmentStaticRotationQuaternion _Output_GetSegmentStaticRotationQuaternion; - Client_GetSegmentStaticRotationQuaternion(pClient, SubjectName, SegmentName, &_Output_GetSegmentStaticRotationQuaternion); - printf( " Static Rotation Quaternion: (%f, %f, %f, %f)\n" - , _Output_GetSegmentStaticRotationQuaternion.Rotation[0] - , _Output_GetSegmentStaticRotationQuaternion.Rotation[1] - , _Output_GetSegmentStaticRotationQuaternion.Rotation[2] - , _Output_GetSegmentStaticRotationQuaternion.Rotation[3] - ); - - // Get the static segment rotation in EulerXYZ co-ordinates - COutput_GetSegmentStaticRotationEulerXYZ _Output_GetSegmentStaticRotationEulerXYZ; - Client_GetSegmentStaticRotationEulerXYZ(pClient, SubjectName, SegmentName, &_Output_GetSegmentStaticRotationEulerXYZ); - printf( " Static Rotation EulerXYZ: (%f, %f, %f)\n" - , _Output_GetSegmentStaticRotationEulerXYZ.Rotation[0] - , _Output_GetSegmentStaticRotationEulerXYZ.Rotation[1] - , _Output_GetSegmentStaticRotationEulerXYZ.Rotation[2] - ); - - // Get the global segment translation - COutput_GetSegmentGlobalTranslation _Output_GetSegmentGlobalTranslation; - Client_GetSegmentGlobalTranslation(pClient, SubjectName, SegmentName, &_Output_GetSegmentGlobalTranslation); - printf( " Global Translation: (%f, %f, %f) %s\n" - , _Output_GetSegmentGlobalTranslation.Translation[0] - , _Output_GetSegmentGlobalTranslation.Translation[1] - , _Output_GetSegmentGlobalTranslation.Translation[2] - , Adapt(_Output_GetSegmentGlobalTranslation.Occluded) - ); - - // Get the global segment rotation in helical co-ordinates - COutput_GetSegmentGlobalRotationHelical _Output_GetSegmentGlobalRotationHelical; - Client_GetSegmentGlobalRotationHelical(pClient, SubjectName, SegmentName, &_Output_GetSegmentGlobalRotationHelical); - printf( " Global Rotation Helical: (%f, %f, %f) %s\n" - , _Output_GetSegmentGlobalRotationHelical.Rotation[0] - , _Output_GetSegmentGlobalRotationHelical.Rotation[1] - , _Output_GetSegmentGlobalRotationHelical.Rotation[2] - , Adapt(_Output_GetSegmentGlobalRotationHelical.Occluded) - ); - - // Get the global segment rotation as a matrix - COutput_GetSegmentGlobalRotationMatrix _Output_GetSegmentGlobalRotationMatrix; - Client_GetSegmentGlobalRotationMatrix(pClient, SubjectName, SegmentName, &_Output_GetSegmentGlobalRotationMatrix); - printf( " Global Rotation Matrix: (%f, %f, %f, %f, %f, %f, %f, %f, %f) %s\n" - , _Output_GetSegmentGlobalRotationMatrix.Rotation[0] - , _Output_GetSegmentGlobalRotationMatrix.Rotation[1] - , _Output_GetSegmentGlobalRotationMatrix.Rotation[2] - , _Output_GetSegmentGlobalRotationMatrix.Rotation[3] - , _Output_GetSegmentGlobalRotationMatrix.Rotation[4] - , _Output_GetSegmentGlobalRotationMatrix.Rotation[5] - , _Output_GetSegmentGlobalRotationMatrix.Rotation[6] - , _Output_GetSegmentGlobalRotationMatrix.Rotation[7] - , _Output_GetSegmentGlobalRotationMatrix.Rotation[8] - , Adapt(_Output_GetSegmentGlobalRotationMatrix.Occluded)); - - // Get the global segment rotation in quaternion co-ordinates - COutput_GetSegmentGlobalRotationQuaternion _Output_GetSegmentGlobalRotationQuaternion; - Client_GetSegmentGlobalRotationQuaternion(pClient, SubjectName, SegmentName, &_Output_GetSegmentGlobalRotationQuaternion); - printf( " Global Rotation Quaternion: (%f, %f, %f, %f) %s\n" - , _Output_GetSegmentGlobalRotationQuaternion.Rotation[0] - , _Output_GetSegmentGlobalRotationQuaternion.Rotation[1] - , _Output_GetSegmentGlobalRotationQuaternion.Rotation[2] - , _Output_GetSegmentGlobalRotationQuaternion.Rotation[3] - , Adapt(_Output_GetSegmentGlobalRotationQuaternion.Occluded)); - - // Get the global segment rotation in EulerXYZ co-ordinates - COutput_GetSegmentGlobalRotationEulerXYZ _Output_GetSegmentGlobalRotationEulerXYZ; - Client_GetSegmentGlobalRotationEulerXYZ(pClient, SubjectName, SegmentName, &_Output_GetSegmentGlobalRotationEulerXYZ); - printf( " Global Rotation EulerXYZ: (%f, %f, %f) %s\n" - , _Output_GetSegmentGlobalRotationEulerXYZ.Rotation[0] - , _Output_GetSegmentGlobalRotationEulerXYZ.Rotation[1] - , _Output_GetSegmentGlobalRotationEulerXYZ.Rotation[2] - , Adapt(_Output_GetSegmentGlobalRotationEulerXYZ.Occluded)); - - // Get the local segment translation - COutput_GetSegmentLocalTranslation _Output_GetSegmentLocalTranslation; - Client_GetSegmentLocalTranslation(pClient, SubjectName, SegmentName, &_Output_GetSegmentLocalTranslation); - printf( " Local Translation: (%f, %f, %f) %s\n" - , _Output_GetSegmentLocalTranslation.Translation[0] - , _Output_GetSegmentLocalTranslation.Translation[1] - , _Output_GetSegmentLocalTranslation.Translation[2] - , Adapt(_Output_GetSegmentLocalTranslation.Occluded)); - - // Get the local segment rotation in helical co-ordinates - COutput_GetSegmentLocalRotationHelical _Output_GetSegmentLocalRotationHelical; - Client_GetSegmentLocalRotationHelical(pClient, SubjectName, SegmentName, &_Output_GetSegmentLocalRotationHelical); - printf( " Local Rotation Helical: (%f, %f, %f) %s\n" - , _Output_GetSegmentLocalRotationHelical.Rotation[0] - , _Output_GetSegmentLocalRotationHelical.Rotation[1] - , _Output_GetSegmentLocalRotationHelical.Rotation[2] - , Adapt(_Output_GetSegmentLocalRotationHelical.Occluded)); - - // Get the local segment rotation as a matrix - COutput_GetSegmentLocalRotationMatrix _Output_GetSegmentLocalRotationMatrix; - Client_GetSegmentLocalRotationMatrix(pClient, SubjectName, SegmentName, &_Output_GetSegmentLocalRotationMatrix); - printf( " Local Rotation Matrix: (%f, %f, %f, %f, %f, %f, %f, %f, %f) %s\n" - , _Output_GetSegmentLocalRotationMatrix.Rotation[0] - , _Output_GetSegmentLocalRotationMatrix.Rotation[1] - , _Output_GetSegmentLocalRotationMatrix.Rotation[2] - , _Output_GetSegmentLocalRotationMatrix.Rotation[3] - , _Output_GetSegmentLocalRotationMatrix.Rotation[4] - , _Output_GetSegmentLocalRotationMatrix.Rotation[5] - , _Output_GetSegmentLocalRotationMatrix.Rotation[6] - , _Output_GetSegmentLocalRotationMatrix.Rotation[7] - , _Output_GetSegmentLocalRotationMatrix.Rotation[8] - , Adapt(_Output_GetSegmentLocalRotationMatrix.Occluded)); - - // Get the local segment rotation in quaternion co-ordinates - COutput_GetSegmentLocalRotationQuaternion _Output_GetSegmentLocalRotationQuaternion; - Client_GetSegmentLocalRotationQuaternion(pClient, SubjectName, SegmentName, &_Output_GetSegmentLocalRotationQuaternion); - printf( " Local Rotation Quaternion: (%f, %f, %f, %f) %s\n" - , _Output_GetSegmentLocalRotationQuaternion.Rotation[0] - , _Output_GetSegmentLocalRotationQuaternion.Rotation[1] - , _Output_GetSegmentLocalRotationQuaternion.Rotation[2] - , _Output_GetSegmentLocalRotationQuaternion.Rotation[3] - , Adapt(_Output_GetSegmentLocalRotationQuaternion.Occluded)); - - // Get the local segment rotation in EulerXYZ co-ordinates - COutput_GetSegmentLocalRotationEulerXYZ _Output_GetSegmentLocalRotationEulerXYZ; - Client_GetSegmentLocalRotationEulerXYZ(pClient, SubjectName, SegmentName, &_Output_GetSegmentLocalRotationEulerXYZ); - printf( " Local Rotation EulerXYZ: (%f, %f, %f) %s\n" - , _Output_GetSegmentLocalRotationEulerXYZ.Rotation[0] - , _Output_GetSegmentLocalRotationEulerXYZ.Rotation[1] - , _Output_GetSegmentLocalRotationEulerXYZ.Rotation[2] - , Adapt(_Output_GetSegmentLocalRotationEulerXYZ.Occluded)); - } - - // Get the quality of the subject (object) if supported - COutput_GetObjectQuality _Output_GetObjectQuality; - Client_GetObjectQuality(pClient, SubjectName, &_Output_GetObjectQuality); - if (_Output_GetObjectQuality.Result == Success) - { - double Quality = _Output_GetObjectQuality.Quality; - printf( " Quality: %f\n", Quality); - } - - // Count the number of markers - COutput_GetMarkerCount MarkerCount; - Client_GetMarkerCount(pClient, SubjectName, &MarkerCount); - printf( " Markers (%d):\n", MarkerCount.MarkerCount); - unsigned int MarkerIndex; - for (MarkerIndex = 0; MarkerIndex < MarkerCount.MarkerCount; ++MarkerIndex) - { - // Get the marker name - char MarkerName[128]; - Client_GetMarkerName(pClient, SubjectName, MarkerIndex, 128, MarkerName); - - // Get the marker parent - char MarkerParentName[128]; - Client_GetMarkerParentName(pClient, SubjectName, MarkerName, 128, MarkerParentName); - - // Get the global marker translation - COutput_GetMarkerGlobalTranslation _Output_GetMarkerGlobalTranslation; - Client_GetMarkerGlobalTranslation(pClient, SubjectName, MarkerName, &_Output_GetMarkerGlobalTranslation); - - printf( " Marker #%d: %s (%f, %f, %f) %s\n" - , MarkerIndex - , MarkerName - , _Output_GetMarkerGlobalTranslation.Translation[0] - , _Output_GetMarkerGlobalTranslation.Translation[1] - , _Output_GetMarkerGlobalTranslation.Translation[2] - , Adapt(_Output_GetMarkerGlobalTranslation.Occluded)); - - if (bReadRayData) - { - COutput_GetMarkerRayContributionCount _Output_GetMarkerRayContributionCount; - Client_GetMarkerRayContributionCount(pClient, SubjectName, MarkerName, &_Output_GetMarkerRayContributionCount); - - if (_Output_GetMarkerRayContributionCount.Result == Success) - { - printf( " Contributed to by: "); - unsigned int ContributionIndex; - for (ContributionIndex = 0; ContributionIndex < _Output_GetMarkerRayContributionCount.RayContributionsCount; ++ContributionIndex) - { - COutput_GetMarkerRayContribution _Output_GetMarkerRayContribution; - Client_GetMarkerRayContribution(pClient, SubjectName, MarkerName, ContributionIndex, &_Output_GetMarkerRayContribution); - printf( "ID:%d Index:%d " - , _Output_GetMarkerRayContribution.CameraID - , _Output_GetMarkerRayContribution.CentroidIndex); - } - printf( "\n" ); - } - } - } - } - - // Get the unlabeled markers - COutput_GetUnlabeledMarkerCount UnlabeledMarkerCount; - Client_GetUnlabeledMarkerCount(pClient, &UnlabeledMarkerCount); - printf( " Unlabeled Markers (%d):\n", UnlabeledMarkerCount.MarkerCount); - unsigned int UnlabeledMarkerIndex; - for (UnlabeledMarkerIndex = 0; UnlabeledMarkerIndex < UnlabeledMarkerCount.MarkerCount; ++UnlabeledMarkerIndex) - { - // Get the global marker translation - COutput_GetUnlabeledMarkerGlobalTranslation _Output_GetUnlabeledMarkerGlobalTranslation; - Client_GetUnlabeledMarkerGlobalTranslation(pClient, UnlabeledMarkerIndex, &_Output_GetUnlabeledMarkerGlobalTranslation); - - printf( " Marker #%d: (%f, %f, %f)\n" - , UnlabeledMarkerIndex - , _Output_GetUnlabeledMarkerGlobalTranslation.Translation[0] - , _Output_GetUnlabeledMarkerGlobalTranslation.Translation[1] - , _Output_GetUnlabeledMarkerGlobalTranslation.Translation[2]); - } - - // Get the labeled markers - COutput_GetLabeledMarkerCount LabeledMarkerCount; - Client_GetLabeledMarkerCount(pClient, &LabeledMarkerCount); - printf(" Labeled Markers (%d):\n", LabeledMarkerCount.MarkerCount); - unsigned int LabeledMarkerIndex; - for (LabeledMarkerIndex = 0; LabeledMarkerIndex < LabeledMarkerCount.MarkerCount; ++LabeledMarkerIndex) - { - // Get the global marker translation - COutput_GetLabeledMarkerGlobalTranslation _Output_GetLabeledMarkerGlobalTranslation; - Client_GetLabeledMarkerGlobalTranslation(pClient, LabeledMarkerIndex, &_Output_GetLabeledMarkerGlobalTranslation); - - printf(" Marker #%d: (%f, %f, %f)\n" - , LabeledMarkerIndex - , _Output_GetLabeledMarkerGlobalTranslation.Translation[0] - , _Output_GetLabeledMarkerGlobalTranslation.Translation[1] - , _Output_GetLabeledMarkerGlobalTranslation.Translation[2]); - } - - // Count the number of devices - COutput_GetDeviceCount DeviceCount; - Client_GetDeviceCount(pClient, &DeviceCount); - printf( " Devices (%d):\n", DeviceCount.DeviceCount); - unsigned int DeviceIndex; - for (DeviceIndex = 0; DeviceIndex < DeviceCount.DeviceCount; ++DeviceIndex) - { - printf( " Device #%d:\n", DeviceIndex); - - // Get the device name and type - char DeviceName[128]; - CEnum DeviceType; //TODO does this need fixing? - Client_GetDeviceName(pClient, DeviceIndex, 128, DeviceName, &DeviceType); - printf( " Name: %s\n", DeviceName); - printf( " Type: %s\n", AdaptDeviceType(DeviceType)); - //TODO Is the device type accessed from the COutput_GetDeviceName struct? - - // Count the number of device outputs - COutput_GetDeviceOutputCount DeviceOutputCount; - Client_GetDeviceOutputCount(pClient, DeviceName, &DeviceOutputCount); - printf( " Device Outputs (%d):\n", DeviceOutputCount.DeviceOutputCount); - unsigned int DeviceOutputIndex; - for (DeviceOutputIndex = 0; DeviceOutputIndex < DeviceOutputCount.DeviceOutputCount; ++DeviceOutputIndex) - { - // Get the device output name and unit - char DeviceOutputName[128]; - CEnum DeviceOutputUnit; //TODO does this need fixing? - Client_GetDeviceOutputName(pClient, DeviceName, DeviceOutputIndex, 128, DeviceOutputName, &DeviceOutputUnit); - - COutput_GetDeviceOutputSubsamples DeviceOutputSubsamples; - Client_GetDeviceOutputSubsamples(pClient, DeviceName, - DeviceOutputName, &DeviceOutputSubsamples); - - printf( " Device Output #%d:\n", DeviceOutputIndex); - printf( " Samples (%d):\n", DeviceOutputSubsamples.DeviceOutputSubsamples); - - unsigned int DeviceOutputSubsample; - for (DeviceOutputSubsample = 0; DeviceOutputSubsample < DeviceOutputSubsamples.DeviceOutputSubsamples; ++DeviceOutputSubsample) - { - printf( " Sample #%d:\n", DeviceOutputSubsample); - - // Get the device output value - //TODO check Client_GetDeviceOutputValue is equivelant to cpp - COutput_GetDeviceOutputValue _Output_GetDeviceOutputValue; - Client_GetDeviceOutputValue(pClient, DeviceName, DeviceOutputName, &_Output_GetDeviceOutputValue); - - printf( " '%s' %f %s %s\n" - , DeviceOutputName - , _Output_GetDeviceOutputValue.Value - , AdaptUnit(DeviceOutputUnit) - , (Adapt(_Output_GetDeviceOutputValue.Occluded))); - } - } - } - - // Output the force plate information. - COutput_GetForcePlateCount ForcePlateCount; - Client_GetForcePlateCount(pClient, &ForcePlateCount); - printf( " Force Plates: (%d)\n", ForcePlateCount.ForcePlateCount); - - unsigned int ForcePlateIndex; - for (ForcePlateIndex = 0; ForcePlateIndex < ForcePlateCount.ForcePlateCount; ++ForcePlateIndex) - { - printf( " Force Plate #%d:\n", ForcePlateIndex); - - COutput_GetForcePlateSubsamples ForcePlateSubsamples; - Client_GetForcePlateSubsamples(pClient, ForcePlateIndex, &ForcePlateSubsamples); - - printf( " Samples (%d):\n", ForcePlateSubsamples.ForcePlateSubsamples); - unsigned int ForcePlateSubsample; - for (ForcePlateSubsample = 0; ForcePlateSubsample < ForcePlateSubsamples.ForcePlateSubsamples; ++ForcePlateSubsample) - { - printf( " Sample #%d:\n", ForcePlateSubsample); - COutput_GetGlobalForceVector _Output_GetForceVector; - Client_GetGlobalForceVectorForSubsample(pClient, ForcePlateIndex, ForcePlateSubsample, &_Output_GetForceVector); - printf( " Force (%f, %f, %f)\n" - , _Output_GetForceVector.ForceVector[0] - , _Output_GetForceVector.ForceVector[1] - , _Output_GetForceVector.ForceVector[2] - ); - COutput_GetGlobalMomentVector _Output_GetMomentVector; - Client_GetGlobalMomentVectorForSubsample(pClient, ForcePlateIndex, ForcePlateSubsample, &_Output_GetMomentVector); - printf( " Moment (%f, %f, %f)\n" - , _Output_GetMomentVector.MomentVector[0] - , _Output_GetMomentVector.MomentVector[1] - , _Output_GetMomentVector.MomentVector[2] - ); - COutput_GetGlobalCentreOfPressure _Output_GetCentreOfPressure; - Client_GetGlobalCentreOfPressureForSubsample(pClient, ForcePlateIndex, ForcePlateSubsample, &_Output_GetCentreOfPressure); - printf( " CoP (%f, %f, %f)\n" - , _Output_GetCentreOfPressure.CentreOfPressure[0] - , _Output_GetCentreOfPressure.CentreOfPressure[1] - , _Output_GetCentreOfPressure.CentreOfPressure[2] - ); - } - } - - // Output eye tracker information. - COutput_GetEyeTrackerCount EyeTrackerCount; - Client_GetEyeTrackerCount(pClient, &EyeTrackerCount); - printf( " Eye Trackers: (%d)\n", EyeTrackerCount.EyeTrackerCount); - unsigned int EyeTrackerIndex; - for (EyeTrackerIndex = 0; EyeTrackerIndex < EyeTrackerCount.EyeTrackerCount; ++EyeTrackerIndex) - { - printf( " Eye Tracker #%d:\n", EyeTrackerIndex); - - COutput_GetEyeTrackerGlobalPosition _Output_GetEyeTrackerGlobalPosition; - Client_GetEyeTrackerGlobalPosition(pClient, EyeTrackerIndex, &_Output_GetEyeTrackerGlobalPosition); - - printf( " Position (%f, %f, %f) %s\n" - , _Output_GetEyeTrackerGlobalPosition.Position[0] - , _Output_GetEyeTrackerGlobalPosition.Position[1] - , _Output_GetEyeTrackerGlobalPosition.Position[2] - , Adapt(_Output_GetEyeTrackerGlobalPosition.Occluded)); - - COutput_GetEyeTrackerGlobalGazeVector _Output_GetEyeTrackerGlobalGazeVector; - Client_GetEyeTrackerGlobalGazeVector(pClient, EyeTrackerIndex, &_Output_GetEyeTrackerGlobalGazeVector); - - printf( " Gaze (%f, %f, %f) %s\n" - , _Output_GetEyeTrackerGlobalGazeVector.GazeVector[0] - , _Output_GetEyeTrackerGlobalGazeVector.GazeVector[1] - , _Output_GetEyeTrackerGlobalGazeVector.GazeVector[2] - , Adapt(_Output_GetEyeTrackerGlobalGazeVector.Occluded) - ); - } - - if (bReadCentroids) - { - COutput_GetCameraCount CameraCount; - Client_GetCameraCount(pClient, &CameraCount); - printf( "Cameras(%d):\n", CameraCount.CameraCount); - unsigned int CameraIndex; - for (CameraIndex = 0; CameraIndex < CameraCount.CameraCount; ++CameraIndex) - { - printf( " Camera #%d:\n", CameraIndex); - - char CameraName[128]; - Client_GetCameraName(pClient, CameraIndex, 128, CameraName); - printf( " Name: %s\n", CameraName); - - COutput_GetCameraId _Output_GetCameraId; - Client_GetCameraId(pClient, CameraName, &_Output_GetCameraId); - printf( " Id: %d\n" , _Output_GetCameraId.CameraId ); - COutput_GetCameraUserId _Output_GetCameraUserId; - Client_GetCameraUserId(pClient, CameraName, &_Output_GetCameraUserId); - printf( " User Id: %d\n" , _Output_GetCameraUserId.CameraUserId ); - char CameraType[128]; - Client_GetCameraType(pClient, CameraName, 128, CameraType); - printf(" Type: %s\n", CameraType); - char CameraDisplayName[128]; - Client_GetCameraDisplayName(pClient, CameraName, 128, CameraDisplayName); - printf(" Display Name: %s\n", CameraDisplayName); - COutput_GetCameraResolution _Output_GetCameraResolution; - Client_GetCameraResolution(pClient, CameraName, &_Output_GetCameraResolution ); - printf( " Resolution: %d x %d\n" , _Output_GetCameraResolution.ResolutionX , _Output_GetCameraResolution.ResolutionY ); - COutput_GetIsVideoCamera _Output_GetIsVideoCamera; - Client_GetIsVideoCamera(pClient, CameraName, &_Output_GetIsVideoCamera); - printf( " Is Video Camera: %s\n" , _Output_GetIsVideoCamera.IsVideoCamera ? "true" : "false"); - - COutput_GetCentroidCount CentroidCount; - Client_GetCentroidCount(pClient, CameraName, &CentroidCount); - printf( " Centroids(%d):\n", CentroidCount.CentroidCount); - unsigned int CentroidIndex; - for (CentroidIndex = 0; CentroidIndex < CentroidCount.CentroidCount; ++CentroidIndex) - { - printf( " Centroid #%d:\n", CentroidIndex); - - COutput_GetCentroidPosition _Output_GetCentroidPosition; - Client_GetCentroidPosition(pClient, CameraName, CentroidIndex, &_Output_GetCentroidPosition); - printf( " Position: (%f, %f)\n" - , _Output_GetCentroidPosition.CentroidPosition[0] - , _Output_GetCentroidPosition.CentroidPosition[1] - ); - printf( " Radius: (%f)\n", _Output_GetCentroidPosition.Radius); - - COutput_GetCentroidWeight _Output_GetCentroidWeight; - Client_GetCentroidWeight(pClient, CameraName, CentroidIndex, &_Output_GetCentroidWeight); - if (_Output_GetCentroidWeight.Result == Success) - { - printf( " Weighting: %f\n", _Output_GetCentroidWeight.Weight); - } - } - } - } - } - - if (EnableMultiCast) - { - Client_StopTransmittingMulticast(pClient); - } - Client_DisableSegmentData(pClient); - Client_DisableMarkerData(pClient); - Client_DisableUnlabeledMarkerData(pClient); - Client_DisableDeviceData(pClient); - if (bReadCentroids) - { - Client_DisableCentroidData(pClient); - } - if (bReadRayData) - { - bReadRayData = 0; - } - - // Disconnect and dispose - int t = clock(); - printf( " Disconnecting...\n" ); - Client_Disconnect(pClient); - int dt = clock() - t; - double secs = (double)(dt) / (double)CLOCKS_PER_SEC; - printf( " Disconnect time = %f secs\n", secs); - - } - - return 0; -} diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CTest/makefile b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CTest/makefile deleted file mode 100644 index bf1ac3385..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CTest/makefile +++ /dev/null @@ -1,107 +0,0 @@ -# Makefile for ViconDataStreamSDK_CTest - -ifndef VERBOSE -.SILENT : -endif -.SUFFIXES : - -ifdef CONFIG -ifneq ($(CONFIG),Debug) -ifneq ($(CONFIG),InternalRelease) -ifneq ($(CONFIG),Release) -Error: unknown configuration. -endif -endif -endif -else -CONFIG=Debug -endif - -INCLUDEPATHS=. ../../../../thirdparty/Boost/boost-1.58.0-dynamic-linux-x64/installed/include ../../../../../deployment\..\vicon\Vicon\CrossMarket\DataStream\ViconDataStreamSDK_CPP ../.. ../ViconDataStreamSDK_C .. -INCLUDEPATHS_Debug=Debug -INCLUDEPATHS_InternalRelease=InternalRelease -INCLUDEPATHS_Release=Release -LIBRARYPATHS=../../../../thirdparty/Boost/boost-1.58.0-dynamic-linux-x64/installed/lib -LIBRARYPATHS_Debug=../../../../lib/Debug ../../../../bin/Debug -LIBRARYPATHS_InternalRelease=../../../../lib/InternalRelease ../../../../bin/InternalRelease -LIBRARYPATHS_Release=../../../../lib/Release ../../../../bin/Release -DEPENDENCIES=ViconDataStreamSDKCore ViconDataStreamSDK_CPP ViconDataStreamSDK_C ViconCGStreamClientSDK ViconCGStreamClient ViconCGStream StreamCommon -LIBRARIES= -LIBRARIES_Debug=boost_wserialization-mt-d boost_wave-mt-d boost_unit_test_framework-mt-d boost_timer-mt-d boost_thread-mt-d boost_system-mt-d boost_signals-mt-d boost_serialization-mt-d boost_regex-mt-d boost_random-mt-d boost_python-mt-d boost_program_options-mt-d boost_prg_exec_monitor-mt-d boost_math_tr1l-mt-d boost_math_tr1f-mt-d boost_math_tr1-mt-d boost_math_c99l-mt-d boost_math_c99f-mt-d boost_math_c99-mt-d boost_log_setup-mt-d boost_log-mt-d boost_locale-mt-d boost_iostreams-mt-d boost_graph-mt-d boost_filesystem-mt-d boost_date_time-mt-d boost_coroutine-mt-d boost_context-mt-d boost_container-mt-d boost_chrono-mt-d boost_atomic-mt-d -LIBRARIES_InternalRelease=boost_wserialization-mt boost_wave-mt boost_unit_test_framework-mt boost_timer-mt boost_thread-mt boost_system-mt boost_signals-mt boost_serialization-mt boost_regex-mt boost_random-mt boost_python-mt boost_program_options-mt boost_prg_exec_monitor-mt boost_math_tr1l-mt boost_math_tr1f-mt boost_math_tr1-mt boost_math_c99l-mt boost_math_c99f-mt boost_math_c99-mt boost_log_setup-mt boost_log-mt boost_locale-mt boost_iostreams-mt boost_graph-mt boost_filesystem-mt boost_date_time-mt boost_coroutine-mt boost_context-mt boost_container-mt boost_chrono-mt boost_atomic-mt -LIBRARIES_Release=boost_wserialization-mt boost_wave-mt boost_unit_test_framework-mt boost_timer-mt boost_thread-mt boost_system-mt boost_signals-mt boost_serialization-mt boost_regex-mt boost_random-mt boost_python-mt boost_program_options-mt boost_prg_exec_monitor-mt boost_math_tr1l-mt boost_math_tr1f-mt boost_math_tr1-mt boost_math_c99l-mt boost_math_c99f-mt boost_math_c99-mt boost_log_setup-mt boost_log-mt boost_locale-mt boost_iostreams-mt boost_graph-mt boost_filesystem-mt boost_date_time-mt boost_coroutine-mt boost_context-mt boost_container-mt boost_chrono-mt boost_atomic-mt -DEFINES=TCM_LINUX TCM_UNIX PROJECT_SOURCE_PATH=\"/home/swtest/workspace/DatastreamSDK_Linux_64_Branch_Release/Binary/bin/Release/deployment/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CTest\" _EXPORTING -DEFINES_Debug=_DEBUG -DEFINES_InternalRelease=VICON_INTERNAL_RELEASE NDEBUG -DEFINES_Release=NDEBUG TCM_OFF_SITE -ENV_CPU=x64 - -SOURCEDIRECTORY=../../../.. -PROJECTPATH=. -BINARYDIRECTORY=../../../.. -INTERMEDIATEDIRECTORY=. -LIBRARYDIRECTORY=../../../../lib -OUTPUTDIRECTORY=../../../../bin - -include $(BINARYDIRECTORY)/gcc.mk - -HIDE_BOOST_SCRIPT=hide_boost_version_script -ifneq ($(HIDE_BOOST),) - HIDE_BOOST_LD_PARAM= -Wl,--version-script=$(HIDE_BOOST_SCRIPT) - HIDE_BOOST_LD_PREREQ=$(HIDE_BOOST_SCRIPT) -endif -all: all_$(CONFIG) - -all_Debug: $(OUTPUTDIRECTORY)/$(CONFIG)/ViconDataStreamSDK_CTest -all_InternalRelease: $(OUTPUTDIRECTORY)/$(CONFIG)/ViconDataStreamSDK_CTest -all_Release: $(OUTPUTDIRECTORY)/$(CONFIG)/ViconDataStreamSDK_CTest - -OBJECTS=$(CONFIG)/ViconDataStreamSDK_CTest.o - -CXXFLAGS=$(CXXFLAGS_$(CONFIG)) $(L_INCLUDEPATHS) $(L_INCLUDEPATHS_$(CONFIG)) $(L_INCLUDES) $(L_INCLUDES_$(CONFIG)) $(L_DEFINES) $(L_DEFINES_$(CONFIG)) -CCFLAGS=$(CCFLAGS_$(CONFIG)) $(L_INCLUDEPATHS) $(L_INCLUDEPATHS_$(CONFIG)) $(L_INCLUDES) $(L_INCLUDES_$(CONFIG)) $(L_DEFINES) $(L_DEFINES_$(CONFIG)) -LDFLAGS=$(LDFLAGS_$(CONFIG)) $(L_LIBRARYPATHS) $(L_LIBRARYPATHS_$(CONFIG)) -CXXFLAGS+=-Wno-unused-local-typedefs -CXXFLAGS+=-Wno-delete-non-virtual-dtor -std=c++14 -#Android toolchain does not include librt but integrates some of its functionality into Android libc. -ifndef ANDROID_TARGET_ARCH -LDFLAGS+=-lrt -endif - - -$(OUTPUTDIRECTORY)/Debug/ViconDataStreamSDK_CTest: makefile $(OBJECTS) $(LIBRARYDIRECTORY)/$(CONFIG)/libStreamCommon.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStream.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClient.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClientSDK.a $(OUTPUTDIRECTORY)/$(CONFIG)/libViconDataStreamSDK_C.so $(OUTPUTDIRECTORY)/$(CONFIG)/libViconDataStreamSDK_CPP.so $(LIBRARYDIRECTORY)/$(CONFIG)/libViconDataStreamSDKCore.a - @echo \[1\;32mLinking EXE $@\ - @mkdir -p $(@D) - $(LD) -Wl,--as-needed -export-dynamic $(LDFLAGS) -o $@ $(OBJECTS) -Wl,--start-group $(L_DEPENDENCIES) $(L_LIBRARIES_$(CONFIG)) $(L_LIBRARIES) -Wl,--end-group -pthread -ldl -Wl,-rpath='$$ORIGIN' -Wl,-rpath-link=.:$(@D) - -$(OUTPUTDIRECTORY)/InternalRelease/ViconDataStreamSDK_CTest: makefile $(OBJECTS) $(LIBRARYDIRECTORY)/$(CONFIG)/libStreamCommon.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStream.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClient.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClientSDK.a $(OUTPUTDIRECTORY)/$(CONFIG)/libViconDataStreamSDK_C.so $(OUTPUTDIRECTORY)/$(CONFIG)/libViconDataStreamSDK_CPP.so $(LIBRARYDIRECTORY)/$(CONFIG)/libViconDataStreamSDKCore.a - @echo \[1\;32mLinking EXE $@\ - @mkdir -p $(@D) - $(LD) -Wl,--as-needed -export-dynamic $(LDFLAGS) -o $@ $(OBJECTS) -Wl,--start-group $(L_DEPENDENCIES) $(L_LIBRARIES_$(CONFIG)) $(L_LIBRARIES) -Wl,--end-group -pthread -ldl -Wl,-rpath='$$ORIGIN' -Wl,-rpath-link=.:$(@D) - -$(OUTPUTDIRECTORY)/Release/ViconDataStreamSDK_CTest: makefile $(OBJECTS) $(LIBRARYDIRECTORY)/$(CONFIG)/libStreamCommon.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStream.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClient.a $(LIBRARYDIRECTORY)/$(CONFIG)/libViconCGStreamClientSDK.a $(OUTPUTDIRECTORY)/$(CONFIG)/libViconDataStreamSDK_C.so $(OUTPUTDIRECTORY)/$(CONFIG)/libViconDataStreamSDK_CPP.so $(LIBRARYDIRECTORY)/$(CONFIG)/libViconDataStreamSDKCore.a - @echo \[1\;32mLinking EXE $@\ - @mkdir -p $(@D) - $(LD) -Wl,--as-needed -export-dynamic $(LDFLAGS) -o $@ $(OBJECTS) -Wl,--start-group $(L_DEPENDENCIES) $(L_LIBRARIES_$(CONFIG)) $(L_LIBRARIES) -Wl,--end-group -pthread -ldl -Wl,-rpath='$$ORIGIN' -Wl,-rpath-link=.:$(@D) - -# Source Files -$(CONFIG)/ViconDataStreamSDK_CTest.o: makefile $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CTest/ViconDataStreamSDK_CTest.c - @echo \[1\;34mCompiling ViconDataStreamSDK_CTest.c\ - @mkdir -p $(@D) - find $(CONFIG) -name *.gch -exec cp '{}' . \; - $(CC) -fPIC -MMD -MP -I$(CONFIG)/ $(CCFLAGS) -o $@ -c $(SOURCEDIRECTORY)/Vicon/CrossMarket/DataStream/ViconDataStreamSDK_CTest/ViconDataStreamSDK_CTest.c - --include $(CONFIG)/ViconDataStreamSDK_CTest.d - -# Other Files - -clean: - @echo \[1\;31mCleaning $(CONFIG) build\ - find . -path '*/$(CONFIG)/*' \( -name '*.[od]' -o -name '*.gch' \) -exec rm -f {} ';' - rm -f moc_*.cxx - -$(HIDE_BOOST_SCRIPT): makefile - echo -n >$@ - echo "{" >>$@ - echo " local: *N5boost*; *NK5boost*;" >>$@ - echo "};" >>$@ diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/StreamCommon/Buffer.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/StreamCommon/Buffer.h deleted file mode 100644 index 633ab0dee..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/StreamCommon/Buffer.h +++ /dev/null @@ -1,250 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -#include "BufferDetail.h" -#include "Type.h" - -#include -#include -#include -#include - -namespace ViconCGStreamIO -{ -//------------------------------------------------------------------------------------------------- - -/// Disable: 'this' : used in base member initializer list -#pragma warning( push ) -#pragma warning( disable : 4355 ) - -/** \class VBuffer -**/ -class VBuffer -{ -public: - - VBuffer( const std::vector< unsigned char > & i_rBuffer = std::vector< unsigned char >() ) - : m_BufferImpl( *this, i_rBuffer ) - { - } - - /// Read value. - template< typename T > - bool Read( T & o_rValue ) const - { - return VBufferDetail< VIsPod< T >::Answer >::Read( m_BufferImpl, o_rValue ); - } - - /// Read fixed sized array. - template< typename T, unsigned int N > - bool Read( T ( & o_pValues )[ N ] ) const - { - return VBufferDetail< VIsPod< T >::Answer >::Read( m_BufferImpl, o_pValues, N ); - } - - /// Read vector. - template< typename T > - bool Read( std::vector< T > & o_rValues ) const - { - o_rValues.clear(); - ViconCGStreamType::UInt32 Size = 0; - if( !m_BufferImpl.ReadPod( Size ) ) - { - return false; - } - o_rValues.resize( Size ); - return VBufferDetail< VIsPod< T >::Answer >::Read( m_BufferImpl, o_rValues.empty() ? 0 : &o_rValues[ 0 ], Size ); - } - - /// Read map. - template< typename K, typename V > - bool Read( std::map< K, V > & o_rValues ) const - { - o_rValues.clear(); - ViconCGStreamType::UInt32 Size = 0; - if( !m_BufferImpl.ReadPod( Size ) ) - { - return false; - } - for( ViconCGStreamType::UInt32 Index = 0; Index != Size; ++Index ) - { - K Key; - if( !Read( Key ) ) - { - return false; - } - V Value; - if( !Read( Value ) ) - { - return false; - } - o_rValues.insert( std::make_pair( Key, Value ) ); - } - return true; - } - - /// Read set. - template< typename T > - bool Read( std::set< T > & o_rValues ) const - { - o_rValues.clear(); - ViconCGStreamType::UInt32 Size = 0; - if( !m_BufferImpl.ReadPod( Size ) ) - { - return false; - } - for( ViconCGStreamType::UInt32 Index = 0; Index != Size; ++Index ) - { - T Value; - if( !Read( Value ) ) - { - return false; - } - o_rValues.insert( Value ); - } - return true; - } - - /// Write value. - template< typename T > - void Write( const T & i_rValue ) - { - VBufferDetail< VIsPod< T >::Answer >::Write( m_BufferImpl, i_rValue ); - } - - /// Write fixed sized array. - template< typename T, unsigned int N > - void Write( const T ( & i_pValues )[ N ] ) - { - VBufferDetail< VIsPod< T >::Answer >::Write( m_BufferImpl, i_pValues, N ); - } - - /// Write vector. - template< typename T > - void Write( const std::vector< T > & i_rValues ) - { - m_BufferImpl.WritePod< ViconCGStreamType::UInt32 >( static_cast< ViconCGStreamType::UInt32 >( i_rValues.size() ) ); - VBufferDetail< VIsPod< T >::Answer >::Write( m_BufferImpl, i_rValues.empty() ? 0 : &i_rValues[ 0 ], static_cast< unsigned int >( i_rValues.size() ) ); - } - - /// Write map. - template< typename K, typename V > - void Write( const std::map< K, V > & i_rValues ) - { - m_BufferImpl.WritePod< ViconCGStreamType::UInt32 >( static_cast< ViconCGStreamType::UInt32 >( i_rValues.size() ) ); - - typename std::map< K, V >::const_iterator It = i_rValues.begin(); - typename std::map< K, V >::const_iterator End = i_rValues.end(); - for( ; It != End; ++It ) - { - Write( ( *It ).first ); - Write( ( *It ).second ); - } - } - - /// Write set. - template< typename T > - void Write( const std::set< T > & i_rValues ) - { - m_BufferImpl.WritePod< ViconCGStreamType::UInt32 >( static_cast< ViconCGStreamType::UInt32 >( i_rValues.size() ) ); - - typename std::set< T >::const_iterator It = i_rValues.begin(); - typename std::set< T >::const_iterator End = i_rValues.end(); - for( ; It != End; ++It ) - { - Write( *It ); - } - } - - /// Get offset into internal buffer. - unsigned int Offset() const - { - return m_BufferImpl.Offset(); - } - - /// Set offset into internal buffer. - void SetOffset( unsigned int i_Offset ) const - { - m_BufferImpl.SetOffset( i_Offset ); - } - - /// Access to internal buffer. - const std::vector< unsigned char > & Data() const - { - return m_BufferImpl.Data(); - } - - /// Access to internal buffer raw. - unsigned char * Raw() - { - return m_BufferImpl.Raw(); - } - - /// Access to internal buffer raw. - const unsigned char * Raw() const - { - return m_BufferImpl.Raw(); - } - - /// Length - unsigned int Length() const - { - return m_BufferImpl.Length(); - } - - /// Set length - void SetLength( unsigned int i_Length ) - { - m_BufferImpl.SetLength( i_Length ); - } - - /// Clear - void Clear() - { - m_BufferImpl.Clear(); - } - - /// Access to buffer impl. Use only for low level work and double check correctness. - const ViconCGStreamIO::VBufferImpl & BufferImpl() const - { - return m_BufferImpl; - } - - /// Access to buffer impl. Use only for low level work and double check correctness. - ViconCGStreamIO::VBufferImpl & BufferImpl() - { - return m_BufferImpl; - } - -private: - ViconCGStreamIO::VBufferImpl m_BufferImpl; -}; - -#pragma warning( pop ) -//------------------------------------------------------------------------------------------------- -}; - - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/StreamCommon/BufferDetail.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/StreamCommon/BufferDetail.h deleted file mode 100644 index 624b75e51..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/StreamCommon/BufferDetail.h +++ /dev/null @@ -1,226 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -#include "BufferImpl.h" -#include "Type.h" -#include - -namespace ViconCGStreamIO -{ -//------------------------------------------------------------------------------------------------- - -template< typename T > -struct VIsPod -{ - enum { Answer = 0 }; -}; - -template< typename T > -struct VIsPod< const T > -{ - enum { Answer = VIsPod< T >::Answer }; -}; - -template<> -struct VIsPod< bool > -{ - enum { Answer = 1 }; -}; - -template<> -struct VIsPod< char > -{ - enum { Answer = 1 }; -}; - -template<> -struct VIsPod< unsigned char > -{ - enum { Answer = 1 }; -}; - -template<> -struct VIsPod< short > -{ - enum { Answer = 1 }; -}; - -template<> -struct VIsPod< unsigned short > -{ - enum { Answer = 1 }; -}; - -template<> -struct VIsPod< int > -{ - enum { Answer = 1 }; -}; - -template<> -struct VIsPod< unsigned int > -{ - enum { Answer = 1 }; -}; - -template<> -struct VIsPod< long > -{ - enum { Answer = 1 }; -}; - -template<> -struct VIsPod< unsigned long > -{ - enum { Answer = 1 }; -}; - -template<> -struct VIsPod< long long > -{ - enum { Answer = 1 }; -}; - -template<> -struct VIsPod< unsigned long long > -{ - enum { Answer = 1 }; -}; - -template<> -struct VIsPod< float > -{ - enum { Answer = 1 }; -}; - -template<> -struct VIsPod< double > -{ - enum { Answer = 1 }; -}; - -//------------------------------------------------------------------------------------------------- - -template< int IsPod > -class VBufferDetail; - -template<> -class VBufferDetail< 1 > -{ -public: - template< typename T > - static bool Read( const VBufferImpl & i_rBufferImpl, T & o_rValue ) - { - return i_rBufferImpl.ReadPod( o_rValue ); - } - - template< typename T > - static bool Read( const VBufferImpl & i_rBufferImpl, T * o_pValues, unsigned int i_Size ) - { - return i_rBufferImpl.ReadPodArray( o_pValues, i_Size ); - } - - template< typename T > - static void Write( VBufferImpl & i_rBufferImpl, const T & i_rValue ) - { - i_rBufferImpl.WritePod( i_rValue ); - } - - template< typename T > - static void Write( VBufferImpl & i_rBufferImpl, const T * i_pValues, unsigned int i_Size ) - { - i_rBufferImpl.WritePodArray( i_pValues, i_Size ); - } -}; - -template<> -class VBufferDetail< 0 > -{ -public: - - template< typename T > - static bool Read( const VBufferImpl & i_rBufferImpl, T & o_rValue ) - { - return o_rValue.Read( i_rBufferImpl.Parent() ); - } - - template< typename T > - static bool Read( const VBufferImpl & i_rBufferImpl, T * o_pValues, unsigned int i_Size ) - { - for( unsigned int Index = 0; Index != i_Size; ++Index ) - { - if( !Read( i_rBufferImpl, o_pValues[ Index ] ) ) - { - return false; - } - } - return true; - } - - static bool Read( const VBufferImpl & i_rBufferImpl, std::string & o_rValue ) - { - ViconCGStreamType::UInt32 Size = 0; - if( !i_rBufferImpl.ReadPod( Size ) ) - { - return false; - } - const unsigned int Offset = i_rBufferImpl.Offset(); - if( Offset + Size > i_rBufferImpl.Length() ) - { - return false; - } - o_rValue.assign( reinterpret_cast< const char * >( i_rBufferImpl.Raw() + Offset ), Size ); - i_rBufferImpl.SetOffset( Offset + Size ); - return true; - } - - template< typename VBufferImpl, typename T > - static void Write( VBufferImpl & i_rBufferImpl, const T & i_rValue ) - { - i_rValue.Write( i_rBufferImpl.Parent() ); - } - - template< typename VBufferImpl, typename T > - static void Write( VBufferImpl & i_rBufferImpl, const T * i_pValues, unsigned int i_Size ) - { - for( unsigned int Index = 0; Index != i_Size; ++Index ) - { - Write( i_rBufferImpl, i_pValues[ Index ] ); - } - } - - static void Write( VBufferImpl & i_rBufferImpl, const std::string & i_rValue ) - { - i_rBufferImpl.WritePod< ViconCGStreamType::UInt32 >( static_cast< ViconCGStreamType::UInt32 >( i_rValue.size() ) ); - i_rBufferImpl.WritePodArray( i_rValue.c_str(), static_cast< unsigned int >( i_rValue.size() ) ); - } - -}; - -//------------------------------------------------------------------------------------------------- -}; - - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/StreamCommon/BufferImpl.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/StreamCommon/BufferImpl.h deleted file mode 100644 index 083edc015..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/StreamCommon/BufferImpl.h +++ /dev/null @@ -1,182 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -#include -#include -#include -#include - -namespace ViconCGStreamIO -{ -//------------------------------------------------------------------------------------------------- - -class VBuffer; - -/** \class VBufferImpl -**/ -class VBufferImpl -{ -public: - - /// Construct buffer impl - VBufferImpl( VBuffer & i_rParent, const std::vector< unsigned char > & i_rBuffer = std::vector< unsigned char >() ) - : m_rParent( i_rParent ) - , m_Offset( 0 ) - , m_Buffer( i_rBuffer ) - { - } - - /// Write singluar pod. - template< typename T > - void WritePod( const T & i_rValue ) - { - const unsigned int End = m_Offset + sizeof( T ); - if( m_Buffer.size() < End ) - { - m_Buffer.resize( End ); - } - - *reinterpret_cast< T * >( &m_Buffer[ 0 ] + m_Offset ) = i_rValue; - m_Offset += sizeof( T ); - } - - /// Write array of pods. - template< typename T > - void WritePodArray( const T * i_pValue, unsigned int i_Size ) - { - const unsigned int End = m_Offset + sizeof( T ) * i_Size; - if( m_Buffer.size() < End ) - { - m_Buffer.resize( End ); - } - - memcpy( reinterpret_cast< T * >( &m_Buffer[ 0 ] + m_Offset ), i_pValue, sizeof( T ) * i_Size ); - m_Offset += sizeof( T ) * i_Size; - } - - /// Read singluar pod. - template< typename T > - bool ReadPod( T & o_rValue ) const - { - const size_t sizeOfT = sizeof( T ); - - if( m_Offset + sizeOfT > m_Buffer.size() ) - { - return false; - } - - memcpy( &o_rValue, &m_Buffer[ 0 ] + m_Offset, sizeOfT ); - m_Offset += sizeOfT; - return true; - } - - /// Read array of pods. - template< typename T > - bool ReadPodArray( T * o_pValue, unsigned int i_Size ) const - { - if( m_Offset + sizeof( T ) * i_Size > m_Buffer.size() ) - { - return false; - } - - memcpy( o_pValue, &m_Buffer[ 0 ] + m_Offset, sizeof( T ) * i_Size ); - m_Offset += sizeof( T ) * i_Size; - return true; - } - - /// Get offset into internal buffer. - unsigned int Offset() const - { - return m_Offset; - } - - /// Set offset into internal buffer. - void SetOffset( unsigned int i_Offset ) const - { - m_Offset = i_Offset; - } - - /// Access to internal buffer. - const std::vector< unsigned char > & Data() const - { - return m_Buffer; - } - - /// Access to internal buffer raw. - unsigned char * Raw() - { - return m_Buffer.empty() ? 0 : &m_Buffer[ 0 ]; - } - - /// Access to internal buffer raw. - const unsigned char * Raw() const - { - return m_Buffer.empty() ? 0 : &m_Buffer[ 0 ]; - } - - /// Return buffer length. - unsigned int Length() const - { - return static_cast< unsigned int >( m_Buffer.size() ); - } - - /// Set buffer length. - void SetLength( unsigned int i_Length ) - { - m_Buffer.resize( i_Length ); - m_Offset = ( std::min )( m_Offset, i_Length ); - } - - /// Clear buffer. - void Clear() - { - m_Buffer.clear(); - m_Offset = 0; - } - - /// Get parent buffer object. - const VBuffer & Parent() const - { - return m_rParent; - } - - /// Get parent buffer object. - VBuffer & Parent() - { - return m_rParent; - } - -private: - VBuffer & m_rParent; - mutable unsigned int m_Offset; - - std::vector< unsigned char > m_Buffer; -}; - -//------------------------------------------------------------------------------------------------- -}; - - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/StreamCommon/CGStreamAsyncReaderWriter.cpp b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/StreamCommon/CGStreamAsyncReaderWriter.cpp deleted file mode 100644 index fe4035006..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/StreamCommon/CGStreamAsyncReaderWriter.cpp +++ /dev/null @@ -1,114 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// - -#include "CGStreamAsyncReaderWriter.h" -#include -#include - -#ifdef WIN32 -#pragma warning(disable:4503) // warning C4503: decorated name length exceeded... -#endif - -void VCGStreamAsyncReaderWriter::ReadBuffer( boost::asio::ip::tcp::socket& i_rSocket, - ViconCGStreamIO::VBuffer& i_rBuffer, - boost::function< void( bool ) > i_Handler ) -{ - i_rBuffer.SetLength( sizeof( ViconCGStreamType::Enum ) + sizeof( ViconCGStreamType::UInt32 ) ); - i_rBuffer.SetOffset( 0 ); - boost::asio::async_read( i_rSocket, - boost::asio::buffer( i_rBuffer.Raw(), i_rBuffer.Length() ), - boost::bind( &VCGStreamAsyncReaderWriter::OnBufferHeaderRead, _1, _2, - boost::ref( i_rSocket ), boost::ref( i_rBuffer ), i_Handler ) ); -} - -void VCGStreamAsyncReaderWriter::WriteBuffer( boost::asio::ip::tcp::socket& i_rSocket, - const ViconCGStreamIO::VBuffer& i_rBuffer, - boost::function< void( bool ) > i_Handler ) -{ - boost::asio::async_write( i_rSocket, - boost::asio::buffer( i_rBuffer.Raw(), i_rBuffer.Length() ), - boost::bind( &VCGStreamAsyncReaderWriter::OnBufferWritten, _1, _2, i_Handler ) ); -} - -void VCGStreamAsyncReaderWriter::SendBuffer( std::shared_ptr< boost::asio::ip::udp::socket > i_pSocket, - const ViconCGStreamIO::VBuffer& i_rBuffer, - boost::function< void( bool ) > i_Handler ) -{ - i_pSocket->async_send( boost::asio::buffer( i_rBuffer.Raw(), i_rBuffer.Length() ), - boost::bind( &VCGStreamAsyncReaderWriter::OnBufferSent, i_pSocket, _1, _2, i_Handler ) ); -} - -void VCGStreamAsyncReaderWriter::SendBufferTo( std::shared_ptr< boost::asio::ip::udp::socket > i_pSocket, - const ViconCGStreamIO::VBuffer& i_rBuffer, - const boost::asio::ip::udp::endpoint& i_rEndpoint, - boost::function< void( bool ) > i_Handler ) -{ - i_pSocket->async_send_to( boost::asio::buffer( i_rBuffer.Raw(), i_rBuffer.Length() ), i_rEndpoint, - boost::bind( &VCGStreamAsyncReaderWriter::OnBufferSent, i_pSocket, _1, _2, i_Handler ) ); -} - -void VCGStreamAsyncReaderWriter::OnBufferHeaderRead( const boost::system::error_code i_Error, - const std::size_t /*i_BytesRead*/, - boost::asio::ip::tcp::socket& i_rSocket, - ViconCGStreamIO::VBuffer& i_rBuffer, - boost::function< void( bool ) > i_Handler ) -{ - if( i_Error ) - { - i_Handler( false ); - return; - } - - ViconCGStreamType::UInt32 BlockLength; - i_rBuffer.SetOffset( sizeof( ViconCGStreamType::Enum ) ); - i_rBuffer.Read( BlockLength ); - i_rBuffer.SetLength( i_rBuffer.Length() + BlockLength ); - boost::asio::mutable_buffers_1 AsioBuffer( boost::asio::buffer( i_rBuffer.Raw() + i_rBuffer.Offset(), BlockLength ) ); - i_rBuffer.SetOffset( 0 ); - boost::asio::async_read( i_rSocket, AsioBuffer, boost::bind( &VCGStreamAsyncReaderWriter::OnBufferBodyRead, _1, _2, i_Handler ) ); -} - -void VCGStreamAsyncReaderWriter::OnBufferBodyRead( const boost::system::error_code i_Error, - const std::size_t /*i_BytesRead*/, - boost::function< void( bool ) > i_Handler ) -{ - i_Handler( !i_Error ); -} - -void VCGStreamAsyncReaderWriter::OnBufferWritten( const boost::system::error_code i_Error, - const std::size_t /*i_BytesRead*/, - boost::function< void( bool ) > i_Handler ) -{ - i_Handler( !i_Error ); -} - -void VCGStreamAsyncReaderWriter::OnBufferSent( std::shared_ptr< boost::asio::ip::udp::socket > /*i_pSocket*/, - const boost::system::error_code i_Error, - const std::size_t /*i_BytesRead*/, - boost::function< void( bool ) > i_Handler ) -{ - i_Handler( !i_Error ); -} - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/StreamCommon/CGStreamAsyncReaderWriter.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/StreamCommon/CGStreamAsyncReaderWriter.h deleted file mode 100644 index 5b75fd8a5..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/StreamCommon/CGStreamAsyncReaderWriter.h +++ /dev/null @@ -1,88 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - - -#include "Buffer.h" -#include -#include - -/// Class containing function that implement async reading and writing with an asio socket for the CG stream. -class VCGStreamAsyncReaderWriter -{ -public: - - /// Asynchronously Read a CGStream block from the socket into the buffer. - /// Calls the supplied handler with true on success or false on failure. - /// Note that it is the callers responsibility to ensure that the socket - /// the buffer references remain valid until the handler is called. - static void ReadBuffer( boost::asio::ip::tcp::socket& i_rSocket, - ViconCGStreamIO::VBuffer& i_rBuffer, - boost::function< void( bool ) > i_Handler ); - - /// Asynchronously Write a CGStream block from the buffer to the socket. - /// Calls the supplied handler with true on success or false on failure. - /// Note that it is the callers responsibility to ensure that the socket - /// the buffer references remain valid until the handler is called. - static void WriteBuffer( boost::asio::ip::tcp::socket& i_rSocket, - const ViconCGStreamIO::VBuffer& i_rBuffer, - boost::function< void( bool ) > i_Handler ); - - /// Asynchronously Send a CGStream block from the buffer to the socket. - /// Calls the supplied handler with true on success or false on failure. - /// Note that unlike WriteBuffer the lifetime of the UDP socket is protected - /// by the shared_ptr. - static void SendBuffer( std::shared_ptr< boost::asio::ip::udp::socket > i_pSocket, - const ViconCGStreamIO::VBuffer& i_rBuffer, - boost::function< void( bool ) > i_Handler ); - - /// As SendBuffer but foe connectionless sockets - static void SendBufferTo( std::shared_ptr< boost::asio::ip::udp::socket > i_pSocket, - const ViconCGStreamIO::VBuffer& i_rBuffer, - const boost::asio::ip::udp::endpoint& i_rEndpoint, - boost::function< void( bool ) > i_Handler ); - -private: - - static void OnBufferHeaderRead( const boost::system::error_code i_Error, - const std::size_t i_BytesRead, - boost::asio::ip::tcp::socket& i_rSocket, - ViconCGStreamIO::VBuffer& i_rBuffer, - boost::function< void( bool ) > i_Handler ); - - static void OnBufferBodyRead( const boost::system::error_code i_Error, - const std::size_t i_BytesRead, - boost::function< void( bool ) > i_Handler ); - - static void OnBufferWritten( const boost::system::error_code i_Error, - const std::size_t i_BytesRead, - boost::function< void( bool ) > i_Handler ); - - static void OnBufferSent( std::shared_ptr< boost::asio::ip::udp::socket > i_pSocket, - const boost::system::error_code i_Error, - const std::size_t i_BytesRead, - boost::function< void( bool ) > i_Handler ); - -}; diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/StreamCommon/Type.h b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/StreamCommon/Type.h deleted file mode 100644 index 0f3c8d5e6..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/StreamCommon/Type.h +++ /dev/null @@ -1,72 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////// -// MIT License -// -// Copyright (c) 2017 Vicon Motion Systems Ltd -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -////////////////////////////////////////////////////////////////////////////////// -#pragma once - -/// \file -/// Contains the declaration of the ViconCGStreamType namespace. - -/** - * This namespace defines the set of primitive types used by the protocol Readers and Writers, and - * also by the objects defined in the ViconCGStream namespace. - * - * See ViconCGStreamIO::VReader, ViconCGStreamIO::VWriter. - */ -namespace ViconCGStreamType -{ -//------------------------------------------------------------------------------------------------- - -/// Single 8-bit character. -typedef char Char; -/// 8-bit signed integer. -typedef char Int8; -/// 16-bit signed integer. -typedef short Int16; -/// 32-bit signed integer. -typedef int Int32; -/// 64-bit signed integer. -typedef long long Int64; - -/// 8-bit unsigned integer. -typedef unsigned char UInt8; -/// 16-bit unsigned integer. -typedef unsigned short UInt16; -/// 32-bit unsigned integer. -typedef unsigned int UInt32; -/// 64-bit unsigned integer. -typedef unsigned long long UInt64; - -/// 32-bit IEEE-format floating-point number. -typedef float Float; -/// 64-bit IEEE-format floating-point number. -typedef double Double; - -/** This is a special type to distinguish members of the ViconCGStreamEnum namespace from other - * primitives in the stream. - */ -typedef UInt32 Enum; - -//------------------------------------------------------------------------------------------------- -}; - diff --git a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/StreamCommon/makefile b/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/StreamCommon/makefile deleted file mode 100644 index 0e0420f12..000000000 --- a/navigation/qualisys_motion_capture/mocap_vicon/src/vicon_sdk/Vicon/CrossMarket/StreamCommon/makefile +++ /dev/null @@ -1,108 +0,0 @@ -# Makefile for StreamCommon - -ifndef VERBOSE -.SILENT : -endif -.SUFFIXES : - -ifdef CONFIG -ifneq ($(CONFIG),Debug) -ifneq ($(CONFIG),InternalRelease) -ifneq ($(CONFIG),Release) -Error: unknown configuration. -endif -endif -endif -else -CONFIG=Debug -endif - -INCLUDEPATHS=../../../thirdparty/Boost/boost-1.58.0-dynamic-linux-x64/installed/include . .. -INCLUDEPATHS_Debug=Debug -INCLUDEPATHS_InternalRelease=InternalRelease -INCLUDEPATHS_Release=Release -LIBRARYPATHS=../../../thirdparty/Boost/boost-1.58.0-dynamic-linux-x64/installed/lib -LIBRARYPATHS_Debug=../../../lib/Debug ../../../bin/Debug -LIBRARYPATHS_InternalRelease=../../../lib/InternalRelease ../../../bin/InternalRelease -LIBRARYPATHS_Release=../../../lib/Release ../../../bin/Release -DEPENDENCIES= -LIBRARIES= -LIBRARIES_Debug=boost_wserialization-mt-d boost_wave-mt-d boost_unit_test_framework-mt-d boost_timer-mt-d boost_thread-mt-d boost_system-mt-d boost_signals-mt-d boost_serialization-mt-d boost_regex-mt-d boost_random-mt-d boost_python-mt-d boost_program_options-mt-d boost_prg_exec_monitor-mt-d boost_math_tr1l-mt-d boost_math_tr1f-mt-d boost_math_tr1-mt-d boost_math_c99l-mt-d boost_math_c99f-mt-d boost_math_c99-mt-d boost_log_setup-mt-d boost_log-mt-d boost_locale-mt-d boost_iostreams-mt-d boost_graph-mt-d boost_filesystem-mt-d boost_date_time-mt-d boost_coroutine-mt-d boost_context-mt-d boost_container-mt-d boost_chrono-mt-d boost_atomic-mt-d -LIBRARIES_InternalRelease=boost_wserialization-mt boost_wave-mt boost_unit_test_framework-mt boost_timer-mt boost_thread-mt boost_system-mt boost_signals-mt boost_serialization-mt boost_regex-mt boost_random-mt boost_python-mt boost_program_options-mt boost_prg_exec_monitor-mt boost_math_tr1l-mt boost_math_tr1f-mt boost_math_tr1-mt boost_math_c99l-mt boost_math_c99f-mt boost_math_c99-mt boost_log_setup-mt boost_log-mt boost_locale-mt boost_iostreams-mt boost_graph-mt boost_filesystem-mt boost_date_time-mt boost_coroutine-mt boost_context-mt boost_container-mt boost_chrono-mt boost_atomic-mt -LIBRARIES_Release=boost_wserialization-mt boost_wave-mt boost_unit_test_framework-mt boost_timer-mt boost_thread-mt boost_system-mt boost_signals-mt boost_serialization-mt boost_regex-mt boost_random-mt boost_python-mt boost_program_options-mt boost_prg_exec_monitor-mt boost_math_tr1l-mt boost_math_tr1f-mt boost_math_tr1-mt boost_math_c99l-mt boost_math_c99f-mt boost_math_c99-mt boost_log_setup-mt boost_log-mt boost_locale-mt boost_iostreams-mt boost_graph-mt boost_filesystem-mt boost_date_time-mt boost_coroutine-mt boost_context-mt boost_container-mt boost_chrono-mt boost_atomic-mt -DEFINES=TCM_LINUX TCM_UNIX PROJECT_SOURCE_PATH=\"/home/swtest/workspace/DatastreamSDK_Linux_64_Branch_Release/Binary/bin/Release/deployment/Vicon/CrossMarket/StreamCommon\" -DEFINES_Debug=_DEBUG -DEFINES_InternalRelease=VICON_INTERNAL_RELEASE NDEBUG -DEFINES_Release=NDEBUG TCM_OFF_SITE -ENV_CPU=x64 - -SOURCEDIRECTORY=../../.. -PROJECTPATH=. -BINARYDIRECTORY=../../.. -INTERMEDIATEDIRECTORY=. -LIBRARYDIRECTORY=../../../lib -OUTPUTDIRECTORY=../../../bin - -include $(BINARYDIRECTORY)/gcc.mk - -HIDE_BOOST_SCRIPT=hide_boost_version_script -ifneq ($(HIDE_BOOST),) - HIDE_BOOST_LD_PARAM= -Wl,--version-script=$(HIDE_BOOST_SCRIPT) - HIDE_BOOST_LD_PREREQ=$(HIDE_BOOST_SCRIPT) -endif -all: all_$(CONFIG) - -all_Debug: $(LIBRARYDIRECTORY)/$(CONFIG)/libStreamCommon.a -all_InternalRelease: $(LIBRARYDIRECTORY)/$(CONFIG)/libStreamCommon.a -all_Release: $(LIBRARYDIRECTORY)/$(CONFIG)/libStreamCommon.a - -OBJECTS=$(CONFIG)/CGStreamAsyncReaderWriter.o - -CXXFLAGS=$(CXXFLAGS_$(CONFIG)) $(L_INCLUDEPATHS) $(L_INCLUDEPATHS_$(CONFIG)) $(L_INCLUDES) $(L_INCLUDES_$(CONFIG)) $(L_DEFINES) $(L_DEFINES_$(CONFIG)) -CCFLAGS=$(CCFLAGS_$(CONFIG)) $(L_INCLUDEPATHS) $(L_INCLUDEPATHS_$(CONFIG)) $(L_INCLUDES) $(L_INCLUDES_$(CONFIG)) $(L_DEFINES) $(L_DEFINES_$(CONFIG)) -LDFLAGS=$(LDFLAGS_$(CONFIG)) $(L_LIBRARYPATHS) $(L_LIBRARYPATHS_$(CONFIG)) -CXXFLAGS+=-Wno-unused-local-typedefs -CXXFLAGS+=-Wno-delete-non-virtual-dtor -std=c++14 -#Android toolchain does not include librt but integrates some of its functionality into Android libc. -ifndef ANDROID_TARGET_ARCH -LDFLAGS+=-lrt -endif - - -$(LIBRARYDIRECTORY)/Debug/libStreamCommon.a: makefile $(OBJECTS) - @echo \[1\;35mLinking archive library $@\ - @mkdir -p $(@D) - $(AR) rcs $@ $(OBJECTS) - -$(LIBRARYDIRECTORY)/InternalRelease/libStreamCommon.a: makefile $(OBJECTS) - @echo \[1\;35mLinking archive library $@\ - @mkdir -p $(@D) - $(AR) rcs $@ $(OBJECTS) - -$(LIBRARYDIRECTORY)/Release/libStreamCommon.a: makefile $(OBJECTS) - @echo \[1\;35mLinking archive library $@\ - @mkdir -p $(@D) - $(AR) rcs $@ $(OBJECTS) - -# Source Files -$(CONFIG)/CGStreamAsyncReaderWriter.o: makefile $(SOURCEDIRECTORY)/Vicon/CrossMarket/StreamCommon/CGStreamAsyncReaderWriter.cpp - @echo \[1\;34mCompiling CGStreamAsyncReaderWriter.cpp\ - @mkdir -p $(@D) - find $(CONFIG) -name *.gch -exec cp '{}' . \; - $(CXX) -fPIC -MMD -MP -I$(CONFIG)/ $(CXXFLAGS) -o $@ -c $(SOURCEDIRECTORY)/Vicon/CrossMarket/StreamCommon/CGStreamAsyncReaderWriter.cpp - --include $(CONFIG)/CGStreamAsyncReaderWriter.d - -# Header Files -# Other Files - -clean: - @echo \[1\;31mCleaning $(CONFIG) build\ - find . -path '*/$(CONFIG)/*' \( -name '*.[od]' -o -name '*.gch' \) -exec rm -f {} ';' - rm -f moc_*.cxx - -$(HIDE_BOOST_SCRIPT): makefile - echo -n >$@ - echo "{" >>$@ - echo " local: *N5boost*; *NK5boost*;" >>$@ - echo "};" >>$@ diff --git a/navigation/sensor_interface/scripts/bno055_interface.py b/navigation/sensor_interface/scripts/bno055_interface.py index 7745c0206..40bc10db9 100755 --- a/navigation/sensor_interface/scripts/bno055_interface.py +++ b/navigation/sensor_interface/scripts/bno055_interface.py @@ -15,74 +15,83 @@ class Bno055InterfaceNode(object): def __init__(self): - rospy.init_node('imu_node') + rospy.init_node("imu_node") self.pub_euler = rospy.Publisher( - 'sensors/imu/euler', - Vector3Stamped, - queue_size=1) + "sensors/imu/euler", Vector3Stamped, queue_size=1 + ) self.pub_diagnostics = rospy.Publisher( - 'sensors/imu/diagnostics', - DiagnosticStatus, - queue_size=1) - self.pub_imu = rospy.Publisher( - 'sensors/imu/data', - Imu, - queue_size=1) + "sensors/imu/diagnostics", DiagnosticStatus, queue_size=1 + ) + self.pub_imu = rospy.Publisher("sensors/imu/data", Imu, queue_size=1) self.srv_save_calibration = rospy.Service( - 'sensors/imu/save_calibration', - SaveImuCalibration, - self.save_calibration) + "sensors/imu/save_calibration", SaveImuCalibration, self.save_calibration + ) self.srv_load_calibration = rospy.Service( - 'sensors/imu/load_calibration', - LoadImuCalibration, - self.load_calibration) + "sensors/imu/load_calibration", LoadImuCalibration, self.load_calibration + ) try: - mode_name = rospy.get_param('sensors/bno055/mode') + mode_name = rospy.get_param("sensors/bno055/mode") except KeyError: - rospy.logerr('Could not read mode parameter, defaulting to IMU mode.') - mode_name = 'IMU' + rospy.logerr("Could not read mode parameter, defaulting to IMU mode.") + mode_name = "IMU" - if mode_name == 'IMU': + if mode_name == "IMU": mode = BNO055.OPERATION_MODE_IMUPLUS - elif mode_name == 'NDOF': + elif mode_name == "NDOF": mode = BNO055.OPERATION_MODE_NDOF else: - rospy.logerr('Invalid mode parameter, defaulting to IMU mode.') - mode_name = 'IMU' + rospy.logerr("Invalid mode parameter, defaulting to IMU mode.") + mode_name = "IMU" mode = BNO055.OPERATION_MODE_IMUPLUS try: - self.bno = BNO055.BNO055(rst='P9_12') + self.bno = BNO055.BNO055(rst="P9_12") except RuntimeError: - rospy.logfatal('Unsupported hardware, intiating clean shutdown.') - rospy.signal_shutdown('') + rospy.logfatal("Unsupported hardware, intiating clean shutdown.") + rospy.signal_shutdown("") return if not self.bno.begin(mode): - rospy.logfatal('Failed to initialise BNO055! Is the sensor connected?') - raise rospy.ROSInitException('Failed to initialise BNO055! Is the sensor connected?') + rospy.logfatal("Failed to initialise BNO055! Is the sensor connected?") + raise rospy.ROSInitException( + "Failed to initialise BNO055! Is the sensor connected?" + ) self.status, self.self_test, self.error = self.bno.get_system_status() rospy.logdebug("System status: %s", self.status) rospy.logdebug("Self test result (0x0F is normal): 0x%02X", self.self_test) if self.status == 0x01: - rospy.logwarn("System status: 0x%02X\n (see datasheet section 4.3.59).", self.status) - - self.sw_v, \ - self.bootloader_v, \ - self.accelerometer_id, \ - self.magnetometer_id, \ - self.gyro_id = self.bno.get_revision() - - rospy.logdebug(("Software version: %s\n" "Bootloader version: %s\n" - "Accelerometer ID: 0x%02X\n" "Magnetometer ID: 0x%02X\n" - "Gyroscope ID: 0x%02X\n"), self.sw_v, - self.bootloader_v, self.accelerometer_id, self.accelerometer_id, self.gyro_id) + rospy.logwarn( + "System status: 0x%02X\n (see datasheet section 4.3.59).", self.status + ) + + ( + self.sw_v, + self.bootloader_v, + self.accelerometer_id, + self.magnetometer_id, + self.gyro_id, + ) = self.bno.get_revision() + + rospy.logdebug( + ( + "Software version: %s\n" + "Bootloader version: %s\n" + "Accelerometer ID: 0x%02X\n" + "Magnetometer ID: 0x%02X\n" + "Gyroscope ID: 0x%02X\n" + ), + self.sw_v, + self.bootloader_v, + self.accelerometer_id, + self.accelerometer_id, + self.gyro_id, + ) - rospy.loginfo('Initialized in %s mode.', mode_name) + rospy.loginfo("Initialized in %s mode.", mode_name) self.bno.set_axis_remap( BNO055.AXIS_REMAP_Y, @@ -90,16 +99,21 @@ def __init__(self): BNO055.AXIS_REMAP_Z, BNO055.AXIS_REMAP_POSITIVE, BNO055.AXIS_REMAP_POSITIVE, - BNO055.AXIS_REMAP_POSITIVE + BNO055.AXIS_REMAP_POSITIVE, ) - rospy.logdebug('IMU axis config is {0}'.format(self.bno.get_axis_remap())) + rospy.logdebug("IMU axis config is {0}".format(self.bno.get_axis_remap())) self.talker() def get_diagnostic(self): diag_msg = DiagnosticStatus() - sys_status, gyro_status, accel_status, mag_status = self.bno.get_calibration_status() + ( + sys_status, + gyro_status, + accel_status, + mag_status, + ) = self.bno.get_calibration_status() sys = KeyValue("System status", str(sys_status)) gyro = KeyValue("Gyro calibration status", str(gyro_status)) accel = KeyValue("Accelerometer calibration status", str(accel_status)) @@ -153,8 +167,8 @@ def talker(self): def save_calibration(self, req): values = self.bno.get_calibration() - path = rospkg.RosPack().get_path('sensor_interface') + "/calibration.csv" - with open(path, 'w') as outfile: + path = rospkg.RosPack().get_path("sensor_interface") + "/calibration.csv" + with open(path, "w") as outfile: writer = csv.writer(outfile) writer.writerow(values) @@ -162,7 +176,7 @@ def save_calibration(self, req): return SaveImuCalibrationResponse() def load_calibration(self, req): - path = rospkg.RosPack().get_path('sensor_interface') + "/calibration.csv" + path = rospkg.RosPack().get_path("sensor_interface") + "/calibration.csv" try: with open(path) as infile: reader = csv.reader(infile) @@ -177,11 +191,11 @@ def load_calibration(self, req): return LoadImuCalibrationResponse() -if __name__ == '__main__': +if __name__ == "__main__": try: imu_node = Bno055InterfaceNode() rospy.spin() except IOError: - rospy.logerr('IOError caught, shutting down.') + rospy.logerr("IOError caught, shutting down.") except rospy.ROSInterruptException: pass diff --git a/navigation/sensor_interface/scripts/ms5837.py b/navigation/sensor_interface/scripts/ms5837.py index 0d621953b..92e1c0ce4 100644 --- a/navigation/sensor_interface/scripts/ms5837.py +++ b/navigation/sensor_interface/scripts/ms5837.py @@ -3,7 +3,7 @@ try: import smbus except: - print 'Try sudo apt-get install python-smbus' + print("Try sudo apt-get install python-smbus") from time import sleep @@ -12,8 +12,8 @@ MODEL_30BA = 1 # Oversampling options -OSR_256 = 0 -OSR_512 = 1 +OSR_256 = 0 +OSR_512 = 1 OSR_1024 = 2 OSR_2048 = 3 OSR_4096 = 4 @@ -24,30 +24,30 @@ DENSITY_SALTWATER = 1029 # Conversion factors (from native unit, mbar) -UNITS_Pa = 100.0 -UNITS_hPa = 1.0 -UNITS_kPa = 0.1 -UNITS_mbar = 1.0 -UNITS_bar = 0.001 -UNITS_atm = 0.000986923 -UNITS_Torr = 0.750062 -UNITS_psi = 0.014503773773022 +UNITS_Pa = 100.0 +UNITS_hPa = 1.0 +UNITS_kPa = 0.1 +UNITS_mbar = 1.0 +UNITS_bar = 0.001 +UNITS_atm = 0.000986923 +UNITS_Torr = 0.750062 +UNITS_psi = 0.014503773773022 # Valid units UNITS_Centigrade = 1 -UNITS_Farenheit = 2 -UNITS_Kelvin = 3 +UNITS_Farenheit = 2 +UNITS_Kelvin = 3 class MS5837(object): # Registers - _MS5837_ADDR = 0x76 - _MS5837_RESET = 0x1E - _MS5837_ADC_READ = 0x00 - _MS5837_PROM_READ = 0xA0 - _MS5837_CONVERT_D1_256 = 0x40 - _MS5837_CONVERT_D2_256 = 0x50 + _MS5837_ADDR = 0x76 + _MS5837_RESET = 0x1E + _MS5837_ADC_READ = 0x00 + _MS5837_PROM_READ = 0xA0 + _MS5837_CONVERT_D1_256 = 0x40 + _MS5837_CONVERT_D2_256 = 0x50 def __init__(self, model=MODEL_30BA, bus=4): self._model = model @@ -79,42 +79,50 @@ def init(self): # Read calibration values and CRC for i in range(7): - c = self._bus.read_word_data(self._MS5837_ADDR, self._MS5837_PROM_READ + 2*i) - c = ((c & 0xFF) << 8) | (c >> 8) # SMBus is little-endian for word transfers, we need to swap MSB and LSB + c = self._bus.read_word_data( + self._MS5837_ADDR, self._MS5837_PROM_READ + 2 * i + ) + c = ((c & 0xFF) << 8) | ( + c >> 8 + ) # SMBus is little-endian for word transfers, we need to swap MSB and LSB self._C.append(c) crc = (self._C[0] & 0xF000) >> 12 if crc != self._crc4(self._C): - print "PROM read error, CRC failed!" + print("PROM read error, CRC failed!") return False return True def read(self, oversampling=OSR_8192): if self._bus is None: - print "No bus!" + print("No bus!") return False if oversampling < OSR_256 or oversampling > OSR_8192: - print "Invalid oversampling option!" + print("Invalid oversampling option!") return False # Request D1 conversion (temperature) - self._bus.write_byte(self._MS5837_ADDR, self._MS5837_CONVERT_D1_256 + 2*oversampling) + self._bus.write_byte( + self._MS5837_ADDR, self._MS5837_CONVERT_D1_256 + 2 * oversampling + ) # Maximum conversion time increases linearly with oversampling # max time (seconds) ~= 2.2e-6(x) where x = OSR = (2^8, 2^9, ..., 2^13) # We use 2.5e-6 for some overhead - sleep(2.5e-6 * 2**(8+oversampling)) + sleep(2.5e-6 * 2 ** (8 + oversampling)) d = self._bus.read_i2c_block_data(self._MS5837_ADDR, self._MS5837_ADC_READ, 3) self._D1 = d[0] << 16 | d[1] << 8 | d[2] # Request D2 conversion (pressure) - self._bus.write_byte(self._MS5837_ADDR, self._MS5837_CONVERT_D2_256 + 2*oversampling) + self._bus.write_byte( + self._MS5837_ADDR, self._MS5837_CONVERT_D2_256 + 2 * oversampling + ) # As above - sleep(2.5e-6 * 2**(8+oversampling)) + sleep(2.5e-6 * 2 ** (8 + oversampling)) d = self._bus.read_i2c_block_data(self._MS5837_ADDR, self._MS5837_ADC_READ, 3) self._D2 = d[0] << 16 | d[1] << 8 | d[2] @@ -138,18 +146,18 @@ def pressure(self, conversion=UNITS_mbar): def temperature(self, conversion=UNITS_Centigrade): degC = self._temperature / 100.0 if conversion == UNITS_Farenheit: - return (9/5) * degC + 32 + return (9 / 5) * degC + 32 elif conversion == UNITS_Kelvin: return degC - 273 return degC # Depth relative to MSL pressure in given fluid density def depth(self): - return (self.pressure(UNITS_Pa)-101300)/(self._fluidDensity*9.80665) + return (self.pressure(UNITS_Pa) - 101300) / (self._fluidDensity * 9.80665) # Altitude relative to MSL pressure def altitude(self): - return (1-pow((self.pressure()/1013.25),.190284))*145366.45*.3048 + return (1 - pow((self.pressure() / 1013.25), 0.190284)) * 145366.45 * 0.3048 # Cribbed from datasheet def _calculate(self): @@ -157,78 +165,93 @@ def _calculate(self): SENSi = 0 Ti = 0 - dT = self._D2-self._C[5]*256 + dT = self._D2 - self._C[5] * 256 if self._model == MODEL_02BA: - SENS = self._C[1]*65536+(self._C[3]*dT)/128 - OFF = self._C[2]*131072+(self._C[4]*dT)/64 - self._pressure = (self._D1*SENS/(2097152)-OFF)/(32768) + SENS = self._C[1] * 65536 + (self._C[3] * dT) / 128 + OFF = self._C[2] * 131072 + (self._C[4] * dT) / 64 + self._pressure = (self._D1 * SENS / (2097152) - OFF) / (32768) else: - SENS = self._C[1]*32768+(self._C[3]*dT)/256 - OFF = self._C[2]*65536+(self._C[4]*dT)/128 - self._pressure = (self._D1*SENS/(2097152)-OFF)/(8192) + SENS = self._C[1] * 32768 + (self._C[3] * dT) / 256 + OFF = self._C[2] * 65536 + (self._C[4] * dT) / 128 + self._pressure = (self._D1 * SENS / (2097152) - OFF) / (8192) - self._temperature = 2000+dT*self._C[6]/8388608 + self._temperature = 2000 + dT * self._C[6] / 8388608 # Second order compensation if self._model == MODEL_02BA: - if (self._temperature/100) < 20: # Low temp - Ti = (11*dT*dT)/(34359738368) - OFFi = (31*(self._temperature-2000)*(self._temperature-2000))/8 - SENSi = (63*(self._temperature-2000)*(self._temperature-2000))/32 + if (self._temperature / 100) < 20: # Low temp + Ti = (11 * dT * dT) / (34359738368) + OFFi = ( + 31 * (self._temperature - 2000) * (self._temperature - 2000) + ) / 8 + SENSi = ( + 63 * (self._temperature - 2000) * (self._temperature - 2000) + ) / 32 else: - if (self._temperature/100) < 20: # Low temp - Ti = (3*dT*dT)/(8589934592) - OFFi = (3*(self._temperature-2000)*(self._temperature-2000))/2 - SENSi = (5*(self._temperature-2000)*(self._temperature-2000))/8 - if (self._temperature/100) < -15: # Very low temp - OFFi = OFFi+7*(self._temperature+1500l)*(self._temperature+1500) - SENSi = SENSi+4*(self._temperature+1500l)*(self._temperature+1500) - elif (self._temperature/100) >= 20: # High temp - Ti = 2*(dT*dT)/(137438953472) - OFFi = (1*(self._temperature-2000)*(self._temperature-2000))/16 + if (self._temperature / 100) < 20: # Low temp + Ti = (3 * dT * dT) / (8589934592) + OFFi = (3 * (self._temperature - 2000) * (self._temperature - 2000)) / 2 + SENSi = ( + 5 * (self._temperature - 2000) * (self._temperature - 2000) + ) / 8 + if (self._temperature / 100) < -15: # Very low temp + + OFFi = OFFi + 7 * (self._temperature + 1500) * ( + self._temperature + 1500 + ) + SENSi = SENSi + 4 * (self._temperature + 1500) * ( + self._temperature + 1500 + ) + elif (self._temperature / 100) >= 20: # High temp + Ti = 2 * (dT * dT) / (137438953472) + OFFi = ( + 1 * (self._temperature - 2000) * (self._temperature - 2000) + ) / 16 SENSi = 0 - OFF2 = OFF-OFFi - SENS2 = SENS-SENSi + OFF2 = OFF - OFFi + SENS2 = SENS - SENSi if self._model == MODEL_02BA: - self._temperature = (self._temperature-Ti) - self._pressure = (((self._D1*SENS2)/2097152-OFF2)/32768)/100.0 + self._temperature = self._temperature - Ti + self._pressure = (((self._D1 * SENS2) / 2097152 - OFF2) / 32768) / 100.0 else: - self._temperature = (self._temperature-Ti) - self._pressure = (((self._D1*SENS2)/2097152-OFF2)/8192)/10.0 + self._temperature = self._temperature - Ti + self._pressure = (((self._D1 * SENS2) / 2097152 - OFF2) / 8192) / 10.0 # Cribbed from datasheet def _crc4(self, n_prom): n_rem = 0 - n_prom[0] = ((n_prom[0]) & 0x0FFF) + n_prom[0] = (n_prom[0]) & 0x0FFF n_prom.append(0) for i in range(16): - if i%2 == 1: - n_rem ^= ((n_prom[i>>1]) & 0x00FF) + if i % 2 == 1: + n_rem ^= (n_prom[i >> 1]) & 0x00FF else: - n_rem ^= (n_prom[i>>1] >> 8) + n_rem ^= n_prom[i >> 1] >> 8 - for n_bit in range(8,0,-1): + for n_bit in range(8, 0, -1): if n_rem & 0x8000: n_rem = (n_rem << 1) ^ 0x3000 else: - n_rem = (n_rem << 1) + n_rem = n_rem << 1 - n_rem = ((n_rem >> 12) & 0x000F) + n_rem = (n_rem >> 12) & 0x000F self.n_prom = n_prom self.n_rem = n_rem return n_rem ^ 0x00 + class MS5837_30BA(MS5837): def __init__(self, bus=1): MS5837.__init__(self, MODEL_30BA, bus) + class MS5837_02BA(MS5837): def __init__(self, bus=1): MS5837.__init__(self, MODEL_02BA, bus) diff --git a/navigation/sensor_interface/scripts/ms5837_interface.py b/navigation/sensor_interface/scripts/ms5837_interface.py index 271fb8c09..d292e0244 100755 --- a/navigation/sensor_interface/scripts/ms5837_interface.py +++ b/navigation/sensor_interface/scripts/ms5837_interface.py @@ -8,26 +8,24 @@ class Ms5837InterfaceNode(object): def __init__(self): - rospy.init_node('pressure_node') + rospy.init_node("pressure_node") self.pub_pressure = rospy.Publisher( - 'sensors/pressure', - FluidPressure, - queue_size=1) + "sensors/pressure", FluidPressure, queue_size=1 + ) self.pub_temp = rospy.Publisher( - 'sensors/temperature', - Temperature, - queue_size=1) + "sensors/temperature", Temperature, queue_size=1 + ) self.ms5837 = ms5837.MS5837(model=ms5837.MODEL_30BA) if not self.ms5837.init(): - rospy.logfatal('Failed to initialise MS5837! Is the sensor connected?') + rospy.logfatal("Failed to initialise MS5837! Is the sensor connected?") else: if not self.ms5837.read(): - rospy.logfatal('Failed to read MS5837!') + rospy.logfatal("Failed to read MS5837!") else: - rospy.loginfo('Successfully initialised MS5837') + rospy.loginfo("Successfully initialised MS5837") self.talker() def talker(self): @@ -47,16 +45,16 @@ def talker(self): self.pub_pressure.publish(pressure_msg) self.pub_temp.publish(temp_msg) else: - rospy.roswarn('Failed to read pressure sensor!') + rospy.roswarn("Failed to read pressure sensor!") rospy.Rate(10).sleep() -if __name__ == '__main__': +if __name__ == "__main__": try: pressure_node = Ms5837InterfaceNode() rospy.spin() except IOError: - rospy.logerr('IOError caught, shutting down.') + rospy.logerr("IOError caught, shutting down.") except rospy.ROSInterruptException: pass diff --git a/navigation/simple_odometry/include/simple_odometry/simple_odom.h b/navigation/simple_odometry/include/simple_odometry/simple_odom.h index 7b4d285c4..a06fc82dc 100644 --- a/navigation/simple_odometry/include/simple_odometry/simple_odom.h +++ b/navigation/simple_odometry/include/simple_odometry/simple_odom.h @@ -1,36 +1,35 @@ #ifndef SIMPLE_ODOM_H #define SIMPLE_ODOM_H -#include -#include -#include -#include -#include +#include +#include #include #include -#include -#include +#include +#include +#include +#include +#include #include #include -#include -#include +#include +#include /** * @brief Class that combines measurements from IMU and DVL into a simple * odometry publisher. Positions in x and y are estimated by euler integration. * */ -class SimpleOdom -{ +class SimpleOdom { public: SimpleOdom(ros::NodeHandle nh); void spin(); EIGEN_MAKE_ALIGNED_OPERATOR_NEW private: - void imuCallback(const sensor_msgs::Imu& imu_msg); - void dvlCallback(const geometry_msgs::TwistWithCovarianceStamped& twist_msg); - void mocapCallback(const geometry_msgs::PoseStamped& msg); + void imuCallback(const sensor_msgs::Imu &imu_msg); + void dvlCallback(const geometry_msgs::TwistWithCovarianceStamped &twist_msg); + void mocapCallback(const geometry_msgs::PoseStamped &msg); Eigen::Vector3d position; tf2::Quaternion orientation; tf2::Vector3 linear_vel; diff --git a/navigation/simple_odometry/src/simple_odom.cpp b/navigation/simple_odometry/src/simple_odom.cpp old mode 100755 new mode 100644 index 56505e512..0435ea432 --- a/navigation/simple_odometry/src/simple_odom.cpp +++ b/navigation/simple_odometry/src/simple_odom.cpp @@ -1,7 +1,6 @@ #include "simple_odometry/simple_odom.h" -SimpleOdom::SimpleOdom(ros::NodeHandle nh) : nh(nh) -{ +SimpleOdom::SimpleOdom(ros::NodeHandle nh) : nh(nh) { // get params std::string imu_topic; std::string dvl_topic; @@ -12,7 +11,7 @@ SimpleOdom::SimpleOdom(ros::NodeHandle nh) : nh(nh) if (!nh.getParam("simple_odom/imu_topic", imu_topic)) imu_topic = "/auv/imu"; if (!nh.getParam("simple_odom/dvl_topic", dvl_topic)) - dvl_topic = "/auv/odom"; + dvl_topic = "/auv/odom"; if (!nh.getParam("simple_odom/mocap_topic", mocap_topic)) mocap_topic = "/qualisys/Body_1/pose"; if (!nh.getParam("simple_odom/odom_topic", odom_topic)) @@ -29,8 +28,10 @@ SimpleOdom::SimpleOdom(ros::NodeHandle nh) : nh(nh) tf2_ros::TransformListener tf_listener(tf_buffer); double timeout = 10; // seconds to wait for transforms to become available ROS_INFO("Waiting for IMU and DVL transforms.."); - geometry_msgs::TransformStamped imu_transform = tf_buffer.lookupTransform("base_link", imu_link, ros::Time(0), ros::Duration(timeout)); - geometry_msgs::TransformStamped dvl_transform = tf_buffer.lookupTransform("base_link", dvl_link, ros::Time(0), ros::Duration(timeout)); + geometry_msgs::TransformStamped imu_transform = tf_buffer.lookupTransform( + "base_link", imu_link, ros::Time(0), ros::Duration(timeout)); + geometry_msgs::TransformStamped dvl_transform = tf_buffer.lookupTransform( + "base_link", dvl_link, ros::Time(0), ros::Duration(timeout)); tf2::fromMsg(imu_transform.transform.rotation, imu_rotation); tf2::fromMsg(dvl_transform.transform.rotation, dvl_rotation); tf2::fromMsg(dvl_transform.transform.translation, dvl_translation); @@ -44,23 +45,22 @@ SimpleOdom::SimpleOdom(ros::NodeHandle nh) : nh(nh) // wait for first imu, dvl and mocap msg ROS_INFO("Waiting for initial IMU, DVL and MOCAP msgs.."); ros::topic::waitForMessage(imu_topic, nh); - ros::topic::waitForMessage(dvl_topic, nh); + ros::topic::waitForMessage( + dvl_topic, nh); ros::topic::waitForMessage(mocap_topic, nh); ROS_INFO("SimpleOdom initialized"); } -void SimpleOdom::spin() -{ +void SimpleOdom::spin() { ros::Rate rate(update_rate); - while (ros::ok()) - { + while (ros::ok()) { // execute waiting callbacks ros::spinOnce(); // create odom msg - nav_msgs::Odometry odometry_msg; + nav_msgs::Odometry odometry_msg; odometry_msg.pose.pose.position = tf2::toMsg(position); odometry_msg.pose.pose.orientation = tf2::toMsg(orientation); @@ -80,25 +80,24 @@ void SimpleOdom::spin() } } -void SimpleOdom::imuCallback(const sensor_msgs::Imu& imu_msg) -{ +void SimpleOdom::imuCallback(const sensor_msgs::Imu &imu_msg) { tf2::Vector3 angular_vel_imu; tf2::fromMsg(imu_msg.angular_velocity, angular_vel_imu); angular_vel = tf2::quatRotate(imu_rotation, angular_vel_imu); } -void SimpleOdom::dvlCallback(const geometry_msgs::TwistWithCovarianceStamped& twist_msg) -{ +void SimpleOdom::dvlCallback( + const geometry_msgs::TwistWithCovarianceStamped &twist_msg) { tf2::Vector3 linear_vel_dvl, linear_vel_uncorrected; tf2::fromMsg(twist_msg.twist.twist.linear, linear_vel_dvl); linear_vel_uncorrected = tf2::quatRotate(dvl_rotation, linear_vel_dvl); // compensate for translation of DVL - linear_vel = linear_vel_uncorrected + angular_vel.cross(dvl_translation); // from fossen2021 eq 14.3 + linear_vel = linear_vel_uncorrected + + angular_vel.cross(dvl_translation); // from fossen2021 eq 14.3 } -void SimpleOdom::mocapCallback(const geometry_msgs::PoseStamped& msg) -{ +void SimpleOdom::mocapCallback(const geometry_msgs::PoseStamped &msg) { tf2::fromMsg(msg.pose.position, position); tf2::fromMsg(msg.pose.orientation, orientation); } diff --git a/navigation/simple_odometry/src/simple_odom_node.cpp b/navigation/simple_odometry/src/simple_odom_node.cpp index faa00e7b7..00bedb76b 100644 --- a/navigation/simple_odometry/src/simple_odom_node.cpp +++ b/navigation/simple_odometry/src/simple_odom_node.cpp @@ -1,15 +1,14 @@ #include "simple_odom.h" -int main(int argc, char** argv) -{ - const bool DEBUG_MODE = true; // debug logs are printed to console when true +int main(int argc, char **argv) { + const bool DEBUG_MODE = true; // debug logs are printed to console when true ros::init(argc, argv, "simple_odometry"); ros::NodeHandle nh; - if (DEBUG_MODE) - { - ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug); + if (DEBUG_MODE) { + ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, + ros::console::levels::Debug); ros::console::notifyLoggerLevelsChanged(); } diff --git a/navigation/underwater_odom/include/underwater_odom_node.h b/navigation/underwater_odom/include/underwater_odom_node.h index 5f4468eef..95bf77a0c 100644 --- a/navigation/underwater_odom/include/underwater_odom_node.h +++ b/navigation/underwater_odom/include/underwater_odom_node.h @@ -1,22 +1,21 @@ -/* - Written by Kristoffer Rakstad Solberg & Øyvind Denvik, Students - Copyright (c) 2019 Manta AUV, Vortex NTNU. - All rights reserved. */ +/* + Written by Kristoffer Rakstad Solberg & Øyvind Denvik, Students + Copyright (c) 2019 Manta AUV, Vortex NTNU. + All rights reserved. */ -#include -#include -#include -#include -#include -#include -#include +#include +#include #include #include #include #include -#include -#include - +#include +#include +#include +#include +#include +#include +#include /* Include guard to prevent double declaration of identifiers such as types, enums and static variacles */ @@ -26,44 +25,40 @@ class UnderwaterOdom { - public: - - // Constructor - UnderwaterOdom(); +public: + // Constructor + UnderwaterOdom(); - // Destructor - ~UnderwaterOdom(){}; + // Destructor + ~UnderwaterOdom(){}; - // Functions - void pressureCallback(const sensor_msgs::FluidPressure &msg); - void dvlCallback(const geometry_msgs::TwistWithCovarianceStamped &msg); + // Functions + void pressureCallback(const sensor_msgs::FluidPressure &msg); + void dvlCallback(const geometry_msgs::TwistWithCovarianceStamped &msg); - private: +private: + // Nodehandle - // Nodehandle + ros::NodeHandle nh_; - ros::NodeHandle nh_; - - // Subscribers + // Subscribers - ros::Subscriber fluid_pressure_sub_; - ros::Subscriber dvl_twist_sub_; + ros::Subscriber fluid_pressure_sub_; + ros::Subscriber dvl_twist_sub_; - // Publishers + // Publishers - //ros::Publisher depth_odom_pub_; - ros::Publisher odom_pub_; + // ros::Publisher depth_odom_pub_; + ros::Publisher odom_pub_; - // Variables + // Variables - double atmospheric_pressure; // [kPa] - double water_density; //[kg/m3] - double earth_gravitation; //[m/s2] - - // Messages - nav_msgs::Odometry odom; + double atmospheric_pressure; // [kPa] + double water_density; //[kg/m3] + double earth_gravitation; //[m/s2] + // Messages + nav_msgs::Odometry odom; }; - #endif // __underwaterOdom_ROS_HH__ diff --git a/navigation/underwater_odom/src/underwater_odom_node.cpp b/navigation/underwater_odom/src/underwater_odom_node.cpp index dc935fba1..315000ef1 100644 --- a/navigation/underwater_odom/src/underwater_odom_node.cpp +++ b/navigation/underwater_odom/src/underwater_odom_node.cpp @@ -1,4 +1,4 @@ -/* +/* Written by Kristoffer Rakstad Solberg, Student Copyright (c) 2019 Manta AUV, Vortex NTNU. All rights reserved. */ @@ -6,15 +6,16 @@ #include "underwater_odom_node.h" /* Constructor */ -UnderwaterOdom::UnderwaterOdom(){ +UnderwaterOdom::UnderwaterOdom() { - // Subscribers - fluid_pressure_sub_ = nh_.subscribe("/auv/pressure", 1, &UnderwaterOdom::pressureCallback, this); - dvl_twist_sub_ = nh_.subscribe("/auv/dvl_twist", 1, &UnderwaterOdom::dvlCallback, this); + // Subscribers + fluid_pressure_sub_ = nh_.subscribe("/auv/pressure", 1, + &UnderwaterOdom::pressureCallback, this); + dvl_twist_sub_ = + nh_.subscribe("/auv/dvl_twist", 1, &UnderwaterOdom::dvlCallback, this); // Publishers - odom_pub_ = nh_.advertise("/auv/odom",1); - + odom_pub_ = nh_.advertise("/auv/odom", 1); // values picked from /params/environment_config.yaml if (!nh_.getParam("atmosphere/pressure", atmospheric_pressure)) @@ -25,44 +26,41 @@ UnderwaterOdom::UnderwaterOdom(){ if (!nh_.getParam("/gravity/acceleration", earth_gravitation)) ROS_ERROR("Could not read parameter gravititional acceleration."); - - // headers - odom.header.frame_id = "auv/odom"; - odom.child_frame_id = "auv/base_link"; + // headers + odom.header.frame_id = "auv/odom"; + odom.child_frame_id = "auv/base_link"; } - /* Callback */ -void UnderwaterOdom::pressureCallback(const sensor_msgs::FluidPressure &msg){ +void UnderwaterOdom::pressureCallback(const sensor_msgs::FluidPressure &msg) { - // compute depth - const float gauge_pressure = 1000*msg.fluid_pressure - atmospheric_pressure; - const float depth_meters = gauge_pressure / (water_density * earth_gravitation); - - // update odom pose, ENU - odom.pose.pose.position.z = -depth_meters; + // compute depth + const float gauge_pressure = 1000 * msg.fluid_pressure - atmospheric_pressure; + const float depth_meters = + gauge_pressure / (water_density * earth_gravitation); + // update odom pose, ENU + odom.pose.pose.position.z = -depth_meters; } -void UnderwaterOdom::dvlCallback(const geometry_msgs::TwistWithCovarianceStamped &msg){ +void UnderwaterOdom::dvlCallback( + const geometry_msgs::TwistWithCovarianceStamped &msg) { - // header timestam - odom.header.stamp = ros::Time::now(); + // header timestam + odom.header.stamp = ros::Time::now(); - // update odom twist - odom.twist.twist.linear.x = msg.twist.twist.linear.x; - odom.twist.twist.linear.y = msg.twist.twist.linear.y; - odom.twist.twist.linear.z = msg.twist.twist.linear.z; + // update odom twist + odom.twist.twist.linear.x = msg.twist.twist.linear.x; + odom.twist.twist.linear.y = msg.twist.twist.linear.y; + odom.twist.twist.linear.z = msg.twist.twist.linear.z; - // publish odom - odom_pub_.publish(odom); + // publish odom + odom_pub_.publish(odom); } - /* ROS SPIN*/ -int main(int argc, char **argv) -{ +int main(int argc, char **argv) { ros::init(argc, argv, "odom"); ros::NodeHandle nh; UnderwaterOdom odom; diff --git a/navigation/waypoint_action_client/scripts/load_waypoints.py b/navigation/waypoint_action_client/scripts/load_waypoints.py index e70607ddb..037da65e9 100755 --- a/navigation/waypoint_action_client/scripts/load_waypoints.py +++ b/navigation/waypoint_action_client/scripts/load_waypoints.py @@ -11,57 +11,57 @@ from geometry_msgs.msg import Pose from tf.transformations import quaternion_from_euler - # "atan2" - # y 90 - # 180 | 0 ^ - # <---*----> x rot+ | - # -180 | 0 - # -90 - # +# "atan2" +# y 90 +# 180 | 0 ^ +# <---*----> x rot+ | +# -180 | 0 +# -90 +# -class PrepareWaypoints: - def __init__(self, inertial_frame_id='world'): +class PrepareWaypoints: + def __init__(self, inertial_frame_id="world"): # Waypoints INIT - assert inertial_frame_id in ['world', 'world_enu'] + assert inertial_frame_id in ["world", "world_enu"] self._inertial_frame_id = inertial_frame_id self.waypoints = [] def read_from_file(self, filename): if not os.path.isfile(filename): - print 'Invalid waypoint filename, file', filename + print("Invalid waypoint filename, file: " + str(filename)) return False try: - with open(filename, 'r') as wp_file: + with open(filename, "r") as wp_file: wps = yaml.load(wp_file) # check if .yaml has a list - if isinstance(wps['waypoints'], list): - + if isinstance(wps["waypoints"], list): + # pick every waypoint an insert into array - for wp in wps['waypoints']: - x = wp['point'][0] - y = wp['point'][1] - z = wp['point'][2] - #yaw = wp['R'] - #self.waypoints.append((x,y,z,yaw)) - R = wp['RPY'][0] - P = wp['RPY'][1] - Y = wp['RPY'][2] - self.waypoints.append((x,y,z,R,P,Y)) - #print(self.waypoints) - - except Exception, e: - print'Error when loading the waypoint file' - print str(e) + for wp in wps["waypoints"]: + x = wp["point"][0] + y = wp["point"][1] + z = wp["point"][2] + # yaw = wp['R'] + # self.waypoints.append((x,y,z,yaw)) + R = wp["RPY"][0] + P = wp["RPY"][1] + Y = wp["RPY"][2] + self.waypoints.append((x, y, z, R, P, Y)) + # print(self.waypoints) + + except Exception as e: + print("Error when loading the waypoint file" + str(e)) return False return True - - def delete_waypoint(self,waypoints): + + def delete_waypoint(self, waypoints): # pop first waypoint in list waypoints.pop(0) - + + """ # ROS spin if __name__ == '__main__': diff --git a/navigation/waypoint_action_client/scripts/old/move_base_square.py b/navigation/waypoint_action_client/scripts/old/move_base_square.py index 1b7ff6fd4..d7f689250 100755 --- a/navigation/waypoint_action_client/scripts/old/move_base_square.py +++ b/navigation/waypoint_action_client/scripts/old/move_base_square.py @@ -1,124 +1,127 @@ #!/usr/bin/python3 -import roslib; roslib.load_manifest('waypoint_action_client') +import roslib + +roslib.load_manifest("waypoint_action_client") import rospy import actionlib from actionlib_msgs.msg import * from geometry_msgs.msg import Pose, Point, Quaternion -from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal # from ros-planner/navigation lib +from move_base_msgs.msg import ( + MoveBaseAction, + MoveBaseGoal, +) # from ros-planner/navigation lib from tf.transformations import quaternion_from_euler from visualization_msgs.msg import Marker from math import radians, pi -class WaypointClient(): - def __init__(self): - rospy.init_node('wp_client', anonymous=True) - - # How big is the square we want the robot to navigate? - square_size = rospy.get_param("~square_size", 1.0) #meters - - # Create a list to hold the target quaternions (orientations) - quaternions = list() - # First define the corner orientations as Euler angles - euler_angles = (pi/2, pi, 3*pi/2, 0) +class WaypointClient: + def __init__(self): + rospy.init_node("wp_client", anonymous=True) - # Then convert the angles to quaterions - for angle in euler_angles: - q_angle = quaternion_from_euler(0, 0, angle, axes = 'sxyz') - q = Quaternion(*q_angle) - quaternions.append(q) + # How big is the square we want the robot to navigate? + square_size = rospy.get_param("~square_size", 1.0) # meters - # Create a list to hold the waypoint poses - waypoints = list() + # Create a list to hold the target quaternions (orientations) + quaternions = list() - # Append each of the four waypoints to the list. Each waypoint - # is a pose consisting of a position and orientation in the map frame - waypoints.append(Pose(Point(5.0, 2.0, -3.0), quaternions[0])) - waypoints.append(Pose(Point(15.0, 2.0, -3.0), quaternions[1])) - waypoints.append(Pose(Point(15.0, -10.0, 0.0), quaternions[2])) - waypoints.append(Pose(Point(5.0, -10.0, 0.0), quaternions[3])) + # First define the corner orientations as Euler angles + euler_angles = (pi / 2, pi, 3 * pi / 2, 0) - # Initialize the visualization markers for Rviz - #self.init_markers() + # Then convert the angles to quaterions + for angle in euler_angles: + q_angle = quaternion_from_euler(0, 0, angle, axes="sxyz") + q = Quaternion(*q_angle) + quaternions.append(q) + # Create a list to hold the waypoint poses + waypoints = list() - # set a visualization marker at each waypoint - #for waypoint in waypoints: - # p = Point() - # p = waypoint.position - # self.markers.points.append(p) + # Append each of the four waypoints to the list. Each waypoint + # is a pose consisting of a position and orientation in the map frame + waypoints.append(Pose(Point(5.0, 2.0, -3.0), quaternions[0])) + waypoints.append(Pose(Point(15.0, 2.0, -3.0), quaternions[1])) + waypoints.append(Pose(Point(15.0, -10.0, 0.0), quaternions[2])) + waypoints.append(Pose(Point(5.0, -10.0, 0.0), quaternions[3])) + # Initialize the visualization markers for Rviz + # self.init_markers() - # Subscrive to the move_base action server - self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) + # set a visualization marker at each waypoint + # for waypoint in waypoints: + # p = Point() + # p = waypoint.position + # self.markers.points.append(p) - rospy.on_shutdown(self.shutdown) + # Subscrive to the move_base action server + self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) - rospy.loginfo("Waiting for move_base action server") + rospy.on_shutdown(self.shutdown) - # Wait 60 seconds for the ation server to become available - self.move_base.wait_for_server(rospy.Duration(60)) + rospy.loginfo("Waiting for move_base action server") - rospy.loginfo("Connected to move base server") - rospy.loginfo("Starting navigation test") + # Wait 60 seconds for the ation server to become available + self.move_base.wait_for_server(rospy.Duration(60)) - # Initialize a counter to track waypoints - i = 0 + rospy.loginfo("Connected to move base server") + rospy.loginfo("Starting navigation test") - # Cycle through the four waypoints - while i < 4 and not rospy.is_shutdown(): + # Initialize a counter to track waypoints + i = 0 - #update the marker display - #self.marker_pub.publish(self.markers) + # Cycle through the four waypoints + while i < 4 and not rospy.is_shutdown(): - # Initialize the waypoint goal - goal = MoveBaseGoal() + # update the marker display + # self.marker_pub.publish(self.markers) - # us the map frame to define goals poses - goal.target_pose.header.frame_id = 'base_link' + # Initialize the waypoint goal + goal = MoveBaseGoal() - # Set the time stamp to "now" - goal.target_pose.header.stamp = rospy.Time.now() + # us the map frame to define goals poses + goal.target_pose.header.frame_id = "base_link" - # Set the goal pose to the i-th waypoint - goal.target_pose.pose = waypoints[i] + # Set the time stamp to "now" + goal.target_pose.header.stamp = rospy.Time.now() - # Start the robot moving toward the goal - self.move(goal) + # Set the goal pose to the i-th waypoint + goal.target_pose.pose = waypoints[i] - i += 1 + # Start the robot moving toward the goal + self.move(goal) - def move(self, goal): - # Send the goal pose to the MoveBaseAction server - self.move_base.send_goal(goal) + i += 1 - # Allow 1 minute to get there - finished_within_time = self.move_base.wait_for_result(rospy.Duration(60)) + def move(self, goal): + # Send the goal pose to the MoveBaseAction server + self.move_base.send_goal(goal) - # if we don't get there in time, abort goal - if not finished_within_time: - self.move_base.cancel_goal() - rospy.loginfo("Timed out achieving goal") - else: - # We made it - state = self.move_base.get_state() - if state == GoalStatus.SUCCEEDED: - rospy.loginfo("goal succeeded!") + # Allow 1 minute to get there + finished_within_time = self.move_base.wait_for_result(rospy.Duration(60)) + # if we don't get there in time, abort goal + if not finished_within_time: + self.move_base.cancel_goal() + rospy.loginfo("Timed out achieving goal") + else: + # We made it + state = self.move_base.get_state() + if state == GoalStatus.SUCCEEDED: + rospy.loginfo("goal succeeded!") - def shutdown(self): - rospy.loginfo("Stopping the robot...") + def shutdown(self): + rospy.loginfo("Stopping the robot...") - # Cancel any active goals - self.move_base.cancel_goal() - rospy.sleep(2) + # Cancel any active goals + self.move_base.cancel_goal() + rospy.sleep(2) -if __name__ == '__main__': - try: - node = WaypointClient() - rospy.spin() +if __name__ == "__main__": + try: + node = WaypointClient() + rospy.spin() - except rospy.ROSInterruptException: - rospy.loginfo("Test gone wrong") \ No newline at end of file + except rospy.ROSInterruptException: + rospy.loginfo("Test gone wrong") diff --git a/navigation/waypoint_action_client/scripts/old/static_waypoint_client.py b/navigation/waypoint_action_client/scripts/old/static_waypoint_client.py index 4ffb1a36c..21cc9944a 100755 --- a/navigation/waypoint_action_client/scripts/old/static_waypoint_client.py +++ b/navigation/waypoint_action_client/scripts/old/static_waypoint_client.py @@ -15,12 +15,10 @@ from math import radians, pi -class WaypointClient(): - +class WaypointClient: def __init__(self): - - rospy.init_node('waypoint_client') + rospy.init_node("waypoint_client") # waypoint goal count self.goal_cnt = 0 @@ -29,11 +27,11 @@ def __init__(self): quaternions = list() # First define the corner orientations as Euler angles - euler_angles = (pi/2, pi, 3*pi/2, 0) + euler_angles = (pi / 2, pi, 3 * pi / 2, 0) # Then convert the angles to quaterions for angle in euler_angles: - q_angle = quaternion_from_euler(0, 0, angle, axes = 'sxyz') + q_angle = quaternion_from_euler(0, 0, angle, axes="sxyz") q = Quaternion(*q_angle) quaternions.append(q) @@ -47,8 +45,8 @@ def __init__(self): self.waypoints.append(Pose(Point(8.0, -10.0, -3.0), quaternions[2])) self.waypoints.append(Pose(Point(5.0, -10.0, 0.0), quaternions[3])) - #Create action client - self.client = actionlib.SimpleActionClient('move_base',MoveBaseAction) + # Create action client + self.client = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") wait = self.client.wait_for_server(rospy.Duration(15.0)) if not wait: @@ -59,51 +57,60 @@ def __init__(self): rospy.loginfo("Starting goals achievements ...") self.movebase_client() - # Callback triggered if the controller recieves the goal for the client and processes it def active_cb(self): - rospy.loginfo("Goal pose "+str(self.goal_cnt+1)+" is now being processed by the Action Server...") - + rospy.loginfo( + "Goal pose " + + str(self.goal_cnt + 1) + + " is now being processed by the Action Server..." + ) # Callback triggered by feedback from the controller action server def feedback_cb(self, feedback): - rospy.loginfo("Feedback for goal pose "+str(self.goal_cnt+1)+" received") - + rospy.loginfo("Feedback for goal pose " + str(self.goal_cnt + 1) + " received") # Checks the status message from the server def status_cb(self, status, result): - #uint8 PENDING=0 - #uint8 ACTIVE=1 - #uint8 PREEMPTED=2 - #uint8 SUCCEEDED=3 - #uint8 ABORTED=4 - #uint8 REJECTED=5 - #uint8 PREEMPTING=6 - #uint8 RECALLING=7 - #uint8 RECALLED=8 - #uint8 LOST=9 - - #add a new goal count when goal reached + # uint8 PENDING=0 + # uint8 ACTIVE=1 + # uint8 PREEMPTED=2 + # uint8 SUCCEEDED=3 + # uint8 ABORTED=4 + # uint8 REJECTED=5 + # uint8 PREEMPTING=6 + # uint8 RECALLING=7 + # uint8 RECALLED=8 + # uint8 LOST=9 + + # add a new goal count when goal reached self.goal_cnt += 1 # status PREEMPTED=2 if status == 2: - rospy.loginfo("Goal pose "+str(self.goal_cnt)+" received a cancel request after it started executing, completed execution!") + rospy.loginfo( + "Goal pose " + + str(self.goal_cnt) + + " received a cancel request after it started executing, completed execution!" + ) # status SUCCEEDED=3 if status == 3: - rospy.loginfo("Goal pose "+str(self.goal_cnt)+" reached") + rospy.loginfo("Goal pose " + str(self.goal_cnt) + " reached") - if self.goal_cnt