Merge remote-tracking branch 'origin/cleanup' into main
This commit is contained in:
Rooholla-Khorrambakht
commit
86ec02c107
|
|
@ -1,31 +0,0 @@
|
|||
FROM ros:humble
|
||||
ENV DEBIAN_FRONTEND=noninteractive
|
||||
SHELL ["/bin/bash", "-c"]
|
||||
RUN apt-get update && apt-get install -y -qq --no-install-recommends \
|
||||
libglvnd-dev \
|
||||
libgl1-mesa-dev \
|
||||
libegl1-mesa-dev \
|
||||
libgles2-mesa-dev \
|
||||
libxext6 \
|
||||
libx11-6 \
|
||||
freeglut3-dev \
|
||||
git \
|
||||
python3-pip \
|
||||
ros-humble-rmw-cyclonedds-cpp ros-humble-rosidl-generator-dds-idl \
|
||||
libyaml-cpp-dev \
|
||||
ros-humble-xacro \
|
||||
libboost-all-dev\
|
||||
build-essential \
|
||||
cmake \
|
||||
&& rm -rf /var/lib/apt/lists/*
|
||||
|
||||
|
||||
RUN pip3 install mujoco pin matplotlib
|
||||
|
||||
RUN cd / && git clone https://github.com/unitreerobotics/unitree_ros2 && cd /unitree_ros2/cyclonedds_ws/src && \
|
||||
git clone https://github.com/ros2/rmw_cyclonedds -b humble && git clone https://github.com/eclipse-cyclonedds/cyclonedds -b releases/0.10.x &&\
|
||||
cd .. && colcon build --packages-select cyclonedds && source /opt/ros/humble/setup.bash && colcon build
|
||||
|
||||
# Env vars for the nvidia-container-runtime.
|
||||
ENV NVIDIA_VISIBLE_DEVICES all
|
||||
ENV NVIDIA_DRIVER_CAPABILITIES graphics,utility,compute
|
||||
|
|
@ -1,13 +0,0 @@
|
|||
// https://containers.dev/implementors/json_reference/
|
||||
{
|
||||
"name": "go2py",
|
||||
"dockerComposeFile": "docker-compose.yaml",
|
||||
"workspaceFolder": "/home/Go2Py",
|
||||
"service": "go2py-dev",
|
||||
"remoteUser": "root",
|
||||
"customizations": {
|
||||
"vscode": {
|
||||
"extensions": ["dbaeumer.vscode-eslint"]
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1,17 +0,0 @@
|
|||
version: "3.9"
|
||||
services:
|
||||
go2py-dev:
|
||||
build: .
|
||||
container_name: go2py-dev
|
||||
network_mode: host
|
||||
privileged: true
|
||||
command: bash
|
||||
volumes:
|
||||
- /tmp/.X11-unix:/tmp/.X11-unix
|
||||
- ..:/home/Go2Py
|
||||
environment:
|
||||
- DISPLAY=${DISPLAY}
|
||||
- QT_X11_NO_MITSHM=1
|
||||
runtime: nvidia
|
||||
stdin_open: true
|
||||
tty: true
|
||||
|
|
@ -0,0 +1,10 @@
|
|||
*.dae filter=lfs diff=lfs merge=lfs -text
|
||||
*.jpeg filter=lfs diff=lfs merge=lfs -text
|
||||
*.jpg filter=lfs diff=lfs merge=lfs -text
|
||||
*.obj filter=lfs diff=lfs merge=lfs -text
|
||||
*.pcd filter=lfs diff=lfs merge=lfs -text
|
||||
*.png filter=lfs diff=lfs merge=lfs -text
|
||||
*.so filter=lfs diff=lfs merge=lfs -text
|
||||
*.so.* filter=lfs diff=lfs merge=lfs -text
|
||||
*.stl filter=lfs diff=lfs merge=lfs -text
|
||||
*.STL filter=lfs diff=lfs merge=lfs -text
|
||||
|
|
@ -160,17 +160,18 @@ cython_debug/
|
|||
#.idea/
|
||||
.vscode
|
||||
|
||||
deploy/robot_ws/log
|
||||
deploy/robot_ws/install
|
||||
deploy/robot_ws/build
|
||||
deploy/workspaces/bridge_ws/log
|
||||
deploy/workspaces/bridge_ws/install
|
||||
deploy/workspaces/bridge_ws/build
|
||||
|
||||
deploy/dock_ws/log
|
||||
deploy/dock_ws/install
|
||||
deploy/dock_ws/build
|
||||
deploy/workspaces/hesai_ws/log
|
||||
deploy/workspaces/hesai_ws/install
|
||||
deploy/workspaces/hesai_ws/build
|
||||
|
||||
deploy/nav2_ws/install
|
||||
deploy/nav2_ws/log
|
||||
deploy/workspaces/nav2_ws/install
|
||||
deploy/workspaces/nav2_ws/log
|
||||
|
||||
_isaac_sim
|
||||
.vscode
|
||||
deploy/nav2_ws/maps
|
||||
deploy/sgiaun-umut
|
||||
examples/datasets
|
||||
|
|
@ -1,43 +0,0 @@
|
|||
# FROM isaac_ros_dev-aarch64
|
||||
FROM ros:humble
|
||||
ENV DEBIAN_FRONTEND=noninteractive
|
||||
SHELL ["/bin/bash", "-c"]
|
||||
# uodate and install dependencies
|
||||
RUN apt-get update && apt-get install -y \
|
||||
ros-humble-rmw-cyclonedds-cpp ros-humble-rosidl-generator-dds-idl \
|
||||
ros-humble-realsense2-camera \
|
||||
ros-humble-pointcloud-to-laserscan \
|
||||
libyaml-cpp-dev \
|
||||
# ros-humble-isaac-ros-visual-slam \
|
||||
# ros-humble-isaac-ros-occupancy-grid-localizer\
|
||||
libboost-all-dev\
|
||||
build-essential \
|
||||
cmake \
|
||||
git \
|
||||
&& rm -rf /var/lib/apt/lists/*
|
||||
|
||||
# Cheange the ROS2 RMW to CycloneDDS as instructed by Unitree
|
||||
RUN cd / && git clone https://github.com/unitreerobotics/unitree_ros2 && cd /unitree_ros2/cyclonedds_ws/src && \
|
||||
git clone https://github.com/ros2/rmw_cyclonedds -b humble && git clone https://github.com/eclipse-cyclonedds/cyclonedds -b releases/0.10.x &&\
|
||||
cd .. && colcon build --packages-select cyclonedds && source /opt/ros/humble/setup.bash && colcon build
|
||||
|
||||
# RUN echo "source /opt/ros/humble/setup.bash" >> /usr/local/bin/scripts/workspace-entrypoint.sh
|
||||
# RUN echo "source /unitree_ros2/cyclonedds_ws/install/setup.bash" >> /usr/local/bin/scripts/workspace-entrypoint.sh
|
||||
# RUN echo "export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp" >> /usr/local/bin/scripts/workspace-entrypoint.sh
|
||||
# RUN echo "export CYCLONEDDS_URI='<CycloneDDS><Domain><General><Interfaces> <NetworkInterface name="eth0" priority="default" multicast="default" /> </Interfaces></General></Domain></CycloneDDS>'" >> /usr/local/bin/scripts/workspace-entrypoint.sh
|
||||
|
||||
# copy the go2py ros2 nodes
|
||||
COPY deploy/dock_ws/src /dock_ws/src
|
||||
RUN cd /dock_ws && source /opt/ros/humble/setup.bash && colcon build --symlink-install
|
||||
|
||||
# Compile the C++ hypervisor bridge
|
||||
COPY deploy/dds_bridge /dds_bridge
|
||||
WORKDIR /dds_bridge
|
||||
RUN ./install.sh && mkdir build && cd build && cmake .. && make
|
||||
|
||||
# Copy the script to start the nodes
|
||||
COPY deploy/scripts /root/scripts
|
||||
COPY deploy/launch /root/launch
|
||||
# set the entrypoint to bash
|
||||
# ENTRYPOINT ["/bin/bash"]
|
||||
ENTRYPOINT ["/bin/bash", "/root/scripts/dock_hw_start.sh"]
|
||||
|
|
@ -1,42 +0,0 @@
|
|||
# FROM isaac_ros_dev-aarch64
|
||||
FROM ros:humble
|
||||
ENV DEBIAN_FRONTEND=noninteractive
|
||||
SHELL ["/bin/bash", "-c"]
|
||||
# uodate and install dependencies
|
||||
RUN apt-get update && apt-get install -y \
|
||||
ros-humble-rmw-cyclonedds-cpp ros-humble-rosidl-generator-dds-idl \
|
||||
libyaml-cpp-dev \
|
||||
ros-humble-xacro \
|
||||
# ros-humble-isaac-ros-visual-slam \
|
||||
# ros-humble-isaac-ros-occupancy-grid-localizer\
|
||||
libboost-all-dev\
|
||||
build-essential \
|
||||
cmake \
|
||||
git \
|
||||
&& rm -rf /var/lib/apt/lists/*
|
||||
|
||||
# Cheange the ROS2 RMW to CycloneDDS as instructed by Unitree
|
||||
RUN cd / && git clone https://github.com/unitreerobotics/unitree_ros2 && cd /unitree_ros2/cyclonedds_ws/src && \
|
||||
git clone https://github.com/ros2/rmw_cyclonedds -b humble && git clone https://github.com/eclipse-cyclonedds/cyclonedds -b releases/0.10.x &&\
|
||||
cd .. && colcon build --packages-select cyclonedds && source /opt/ros/humble/setup.bash && colcon build
|
||||
|
||||
# RUN echo "source /opt/ros/humble/setup.bash" >> /usr/local/bin/scripts/workspace-entrypoint.sh
|
||||
# RUN echo "source /unitree_ros2/cyclonedds_ws/install/setup.bash" >> /usr/local/bin/scripts/workspace-entrypoint.sh
|
||||
# RUN echo "export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp" >> /usr/local/bin/scripts/workspace-entrypoint.sh
|
||||
# RUN echo "export CYCLONEDDS_URI='<CycloneDDS><Domain><General><Interfaces> <NetworkInterface name="eth0" priority="default" multicast="default" /> </Interfaces></General></Domain></CycloneDDS>'" >> /usr/local/bin/scripts/workspace-entrypoint.sh
|
||||
|
||||
# copy the go2py ros2 nodes
|
||||
COPY deploy/robot_ws/src /robot_ws/src
|
||||
RUN cd /robot_ws && source /opt/ros/humble/setup.bash && colcon build --symlink-install
|
||||
|
||||
# Compile the C++ hypervisor bridge
|
||||
COPY deploy/dds_bridge /dds_bridge
|
||||
WORKDIR /dds_bridge
|
||||
RUN ./install.sh && mkdir build && cd build && cmake .. && make
|
||||
|
||||
# Copy the script to start the nodes
|
||||
COPY deploy/scripts /root/scripts
|
||||
COPY deploy/launch /root/launch
|
||||
# set the entrypoint to bash
|
||||
# ENTRYPOINT ["/bin/bash"]
|
||||
ENTRYPOINT ["/bin/bash", "/root/scripts/robot_hw_start.sh"]
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:8b306743e3775c2b000f0ba490b99415e388f631731940066354fb8389e08e49
|
||||
size 1867195
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:bb6a2b3f3090c86bc82e1c0728e7d2b96b57c1f6921f0298974fcafb8313a6ef
|
||||
size 1672152
|
||||
|
|
@ -0,0 +1,11 @@
|
|||
#Accelerometer
|
||||
accelerometer_noise_density: 0.004059677310589936
|
||||
accelerometer_random_walk: 7.228500058270378e-05
|
||||
|
||||
#Gyroscope
|
||||
gyroscope_noise_density: 0.0010796822189602655
|
||||
gyroscope_random_walk: 9.093779634369827e-06
|
||||
|
||||
rostopic: '/go2/imu' #Make sure this is correct
|
||||
update_rate: 500.0 #Make sure this is correct
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
Before Width: | Height: | Size: 41 KiB |
|
|
@ -6,7 +6,9 @@ import numpy.linalg as LA
|
|||
from scipy.spatial.transform import Rotation as R
|
||||
|
||||
from cyclonedds.domain import DomainParticipant
|
||||
from Go2Py.unitree_go.msg.dds_ import Go2pyLowCmd_
|
||||
from go2py_messages.msg.dds_ import Go2pyLowCmd_
|
||||
from go2py_messages.msg.dds_ import Go2pyHighCmd_
|
||||
from go2py_messages.msg.dds_ import Go2pyState_
|
||||
from cyclonedds.topic import Topic
|
||||
from cyclonedds.pub import DataWriter
|
||||
|
||||
|
|
@ -14,7 +16,6 @@ from cyclonedds.domain import DomainParticipant
|
|||
from cyclonedds.topic import Topic
|
||||
from cyclonedds.sub import DataReader
|
||||
from cyclonedds.util import duration
|
||||
from Go2Py.unitree_go.msg.dds_ import LowState_
|
||||
from threading import Thread
|
||||
from scipy.spatial.transform import Rotation
|
||||
from Go2Py.joy import xKeySwitch, xRockerBtn
|
||||
|
|
@ -31,8 +32,6 @@ class GO2Real():
|
|||
):
|
||||
assert mode in ['highlevel', 'lowlevel'], "mode should be either 'highlevel' or 'lowlevel'"
|
||||
self.mode = mode
|
||||
if self.mode == 'highlevel':
|
||||
raise NotImplementedError('DDS interface for the highlevel commands is not implemented yet. Please use our ROS2 interface.')
|
||||
self.simulated = False
|
||||
self.prestanding_q = np.array([ 0.0, 1.26186061, -2.5,
|
||||
0.0, 1.25883281, -2.5,
|
||||
|
|
@ -50,56 +49,58 @@ class GO2Real():
|
|||
0.0, 0.75422204, -1.53229916])
|
||||
self.latest_command_stamp = time.time()
|
||||
self.highcmd_topic_name = "rt/go2/twist_cmd"
|
||||
self.lowcmd_topic_name = "rt/go2/lowcmd"
|
||||
self.lowstate_topic_name = "rt/lowstate"
|
||||
self.lowcmd_topic_name = "rt/go2py/low_cmd"
|
||||
self.highcmd_topic_name = "rt/go2py/high_cmd"
|
||||
self.lowstate_topic_name = "rt/go2py/state"
|
||||
|
||||
self.participant = DomainParticipant()
|
||||
self.lowstate_topic = Topic(self.participant, self.lowstate_topic_name, LowState_)
|
||||
self.lowstate_reader = DataReader(self.participant, self.lowstate_topic)
|
||||
self.lowstate_topic = Topic(self.participant, self.lowstate_topic_name, Go2pyState_)
|
||||
self.state_reader = DataReader(self.participant, self.lowstate_topic)
|
||||
|
||||
self.lowcmd_topic = Topic(self.participant, self.lowcmd_topic_name, Go2pyLowCmd_)
|
||||
self.lowcmd_writer = DataWriter(self.participant, self.lowcmd_topic)
|
||||
|
||||
self.highcmd_topic = Topic(self.participant, self.highcmd_topic_name, Go2pyHighCmd_)
|
||||
self.highcmd_writer = DataWriter(self.participant, self.highcmd_topic)
|
||||
self.vx_max = vx_max
|
||||
self.vy_max = vy_max
|
||||
self.P_v_max = np.diag([1 / self.vx_max**2, 1 / self.vy_max**2])
|
||||
self.ωz_max = ωz_max
|
||||
self.ωz_min = -ωz_max
|
||||
|
||||
self.state = None
|
||||
self.setCommands = {'lowlevel':self.setCommandsLow}[self.mode]
|
||||
self.lowstate_thread = Thread(target = self.lowstate_update)
|
||||
self.setCommands = {'lowlevel':self.setCommandsLow,
|
||||
'highlevel':self.setCommandsHigh}[self.mode]
|
||||
self.state_thread = Thread(target = self.state_update)
|
||||
self.running = True
|
||||
self.lowstate_thread.start()
|
||||
self.state_thread.start()
|
||||
|
||||
|
||||
def lowstate_update(self):
|
||||
def state_update(self):
|
||||
"""
|
||||
Retrieve the state of the robot
|
||||
"""
|
||||
while self.running:
|
||||
for msg in self.lowstate_reader.take_iter(timeout=duration(milliseconds=100.)):
|
||||
for msg in self.state_reader.take_iter(timeout=duration(milliseconds=1.)):
|
||||
self.state = msg
|
||||
|
||||
def getIMU(self):
|
||||
accel = self.state.imu_state.accelerometer
|
||||
gyro = self.state.imu_state.gyroscope
|
||||
quat = self.state.imu_state.quaternion
|
||||
rpy = self.state.imu_state.rpy
|
||||
temp = self.state.imu_state.temperature
|
||||
return {'accel':accel, 'gyro':gyro, 'quat':quat, "rpy":rpy, 'temp':temp}
|
||||
accel = self.state.accel
|
||||
gyro = self.state.gyro
|
||||
quat = self.state.quat
|
||||
temp = self.state.imu_temp
|
||||
return {'accel':accel, 'gyro':gyro, 'quat':quat, 'temp':temp}
|
||||
|
||||
def getFootContacts(self):
|
||||
"""Returns the raw foot contact forces"""
|
||||
footContacts = self.state.foot_force
|
||||
return np.array(footContacts)
|
||||
footContacts = self.state.contact
|
||||
return footContacts
|
||||
|
||||
def getJointStates(self):
|
||||
"""Returns the joint angles (q) and velocities (dq) of the robot"""
|
||||
motor_state = np.array([[self.state.motor_state[i].q,
|
||||
self.state.motor_state[i].dq,
|
||||
self.state.motor_state[i].ddq,
|
||||
self.state.motor_state[i].tau_est,
|
||||
self.state.motor_state[i].temperature] for i in range(12)])
|
||||
return {'q':motor_state[:,0],
|
||||
'dq':motor_state[:,1],
|
||||
'ddq':motor_state[:,2],
|
||||
'tau_est':motor_state[:,3],
|
||||
'temperature':motor_state[:,4]}
|
||||
return {'q':self.state.q,
|
||||
'dq':self.state.dq,
|
||||
'tau_est':self.state.tau,
|
||||
'temperature':self.state.motor_temp}
|
||||
|
||||
def getRemoteState(self):
|
||||
"""A method to get the state of the wireless remote control.
|
||||
|
|
@ -149,18 +150,17 @@ class GO2Real():
|
|||
|
||||
def getBatteryState(self):
|
||||
"""Returns the battery percentage of the robot"""
|
||||
batteryState = self.state.bms_state
|
||||
return batteryState.soc
|
||||
return self.state.soc
|
||||
|
||||
def setCommandsHigh(self, v_x, v_y, ω_z, bodyHeight=0.0, footRaiseHeight=0.0, mode=2):
|
||||
self.cmd_watchdog_timer = time.time()
|
||||
_v_x, _v_y, _ω_z = self.clip_velocity(v_x, v_y, ω_z)
|
||||
self.highcmd.header.stamp = self.get_clock().now().to_msg()
|
||||
self.highcmd.header.frame_id = "base_link"
|
||||
self.highcmd.twist.linear.x = _v_x
|
||||
self.highcmd.twist.linear.y = _v_y
|
||||
self.highcmd.twist.angular.z = _ω_z
|
||||
self.highcmd_publisher.publish(self.highcmd)
|
||||
highcmd = Go2pyHighCmd_(
|
||||
_v_x,
|
||||
_v_y,
|
||||
_ω_z
|
||||
)
|
||||
self.highcmd_writer.write(highcmd)
|
||||
|
||||
def setCommandsLow(self, q_des, dq_des, kp, kd, tau_ff):
|
||||
assert q_des.size == dq_des.size == kp.size == kd.size == tau_ff.size == 12, "q, dq, kp, kd, tau_ff should have size 12"
|
||||
|
|
@ -177,11 +177,6 @@ class GO2Real():
|
|||
def close(self):
|
||||
self.running = False
|
||||
|
||||
def check_calf_collision(self, q):
|
||||
self.pin_robot.update(q)
|
||||
in_collision = self.pin_robot.check_calf_collision(q)
|
||||
return in_collision
|
||||
|
||||
def clip_velocity(self, v_x, v_y, ω_z):
|
||||
_v = np.array([[v_x], [v_y]])
|
||||
_scale = np.sqrt(_v.T @ self.P_v_max @ _v)[0, 0]
|
||||
|
|
@ -198,6 +193,6 @@ class GO2Real():
|
|||
|
||||
def getGravityInBody(self):
|
||||
q = self.getIMU()['quat']
|
||||
R = Rotation.from_quat([q[1], q[2], q[3], q[0]]).as_matrix()
|
||||
R = Rotation.from_quat(q).as_matrix()
|
||||
g_in_body = R.T@np.array([0.0, 0.0, -1.0]).reshape(3, 1)
|
||||
return g_in_body
|
||||
|
|
@ -1,266 +0,0 @@
|
|||
import struct
|
||||
import threading
|
||||
import time
|
||||
import numpy as np
|
||||
import numpy.linalg as LA
|
||||
from scipy.spatial.transform import Rotation as R
|
||||
import rclpy
|
||||
import tf2_ros
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSProfile
|
||||
from rclpy.executors import MultiThreadedExecutor
|
||||
from geometry_msgs.msg import TransformStamped
|
||||
from Go2Py.joy import xKeySwitch, xRockerBtn
|
||||
from geometry_msgs.msg import TwistStamped
|
||||
from unitree_go.msg import LowState, Go2pyLowCmd
|
||||
from nav_msgs.msg import Odometry
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
|
||||
|
||||
def ros2_init(args=None):
|
||||
rclpy.init(args=args)
|
||||
|
||||
|
||||
def ros2_close():
|
||||
rclpy.shutdown()
|
||||
|
||||
class ROS2ExecutorManager:
|
||||
"""A class to manage the ROS2 executor. It allows to add nodes and start the executor in a separate thread."""
|
||||
def __init__(self):
|
||||
self.executor = MultiThreadedExecutor()
|
||||
self.nodes = []
|
||||
self.executor_thread = None
|
||||
|
||||
def add_node(self, node: Node):
|
||||
"""Add a new node to the executor."""
|
||||
self.nodes.append(node)
|
||||
self.executor.add_node(node)
|
||||
|
||||
def _run_executor(self):
|
||||
try:
|
||||
self.executor.spin()
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
self.terminate()
|
||||
|
||||
def start(self):
|
||||
"""Start spinning the nodes in a separate thread."""
|
||||
self.executor_thread = threading.Thread(target=self._run_executor)
|
||||
self.executor_thread.start()
|
||||
|
||||
def terminate(self):
|
||||
"""Terminate all nodes and shutdown rclpy."""
|
||||
for node in self.nodes:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
if self.executor_thread:
|
||||
self.executor_thread.join()
|
||||
|
||||
class GO2Real(Node):
|
||||
def __init__(
|
||||
self,
|
||||
mode = 'highlevel', # 'highlevel' or 'lowlevel'
|
||||
vx_max=0.5,
|
||||
vy_max=0.4,
|
||||
ωz_max=0.5,
|
||||
):
|
||||
assert mode in ['highlevel', 'lowlevel'], "mode should be either 'highlevel' or 'lowlevel'"
|
||||
self.simulated = False
|
||||
self.prestanding_q = np.array([ 0.0, 1.26186061, -2.5,
|
||||
0.0, 1.25883281, -2.5,
|
||||
0.0, 1.27193761, -2.6,
|
||||
0.0, 1.27148342, -2.6])
|
||||
|
||||
self.sitting_q = np.array([-0.02495611, 1.26249647, -2.82826662,
|
||||
0.04563564, 1.2505368 , -2.7933557 ,
|
||||
-0.30623949, 1.28283751, -2.82314873,
|
||||
0.26400229, 1.29355574, -2.84276843])
|
||||
|
||||
self.standing_q = np.array([ 0.0, 0.77832842, -1.56065452,
|
||||
0.0, 0.76754963, -1.56634164,
|
||||
0.0, 0.76681757, -1.53601146,
|
||||
0.0, 0.75422204, -1.53229916])
|
||||
self.latest_command_stamp = time.time()
|
||||
self.mode = mode
|
||||
self.node_name = "go2py_highlevel_subscriber"
|
||||
self.highcmd_topic = "/go2/twist_cmd"
|
||||
self.lowcmd_topic = "/go2/lowcmd"
|
||||
self.joint_state_topic = "/go2/joint_states"
|
||||
self.lowstate_topic = "/lowstate"
|
||||
super().__init__(self.node_name)
|
||||
|
||||
self.lowstate_subscriber = self.create_subscription(
|
||||
LowState, self.lowstate_topic, self.lowstate_callback, 1
|
||||
)
|
||||
self.lowcmd_publisher = self.create_publisher(Go2pyLowCmd, self.lowcmd_topic, 1)
|
||||
|
||||
self.odometry_subscriber = self.create_subscription(
|
||||
Odometry, "/utlidar/robot_odom", self.odom_callback, 1
|
||||
)
|
||||
|
||||
self.highcmd_publisher = self.create_publisher(TwistStamped, self.highcmd_topic, 1)
|
||||
self.highcmd = TwistStamped()
|
||||
# create pinocchio robot
|
||||
# self.pin_robot = PinRobot()
|
||||
|
||||
# for velocity clipping
|
||||
self.vx_max = vx_max
|
||||
self.vy_max = vy_max
|
||||
self.P_v_max = np.diag([1 / self.vx_max**2, 1 / self.vy_max**2])
|
||||
self.ωz_max = ωz_max
|
||||
self.ωz_min = -ωz_max
|
||||
self.running = True
|
||||
self.setCommands = {'lowlevel':self.setCommandsLow,
|
||||
'highlevel':self.setCommandsHigh}[self.mode]
|
||||
self.state = None
|
||||
|
||||
def lowstate_callback(self, msg):
|
||||
"""
|
||||
Retrieve the state of the robot
|
||||
"""
|
||||
self.state = msg
|
||||
|
||||
def odom_callback(self, msg):
|
||||
"""
|
||||
Retrieve the odometry of the robot
|
||||
"""
|
||||
self.odom = msg
|
||||
|
||||
def getOdometry(self):
|
||||
"""Returns the odometry of the robot"""
|
||||
stamp = self.odom.header.stamp
|
||||
position = np.array([self.odom.pose.pose.position.x, self.odom.pose.pose.position.y, self.odom.pose.pose.position.z])
|
||||
orientation = np.array([self.odom.pose.pose.orientation.x, self.odom.pose.pose.orientation.y, self.odom.pose.pose.orientation.z, self.odom.pose.pose.orientation.w])
|
||||
stamp_nanosec = stamp.sec + stamp.nanosec * 1e-9
|
||||
return {'stamp_nanosec':stamp_nanosec, 'position':position, 'orientation':orientation}
|
||||
|
||||
def getIMU(self):
|
||||
accel = self.state.imu_state.accelerometer
|
||||
gyro = self.state.imu_state.gyroscope
|
||||
quat = self.state.imu_state.quaternion
|
||||
rpy = self.state.imu_state.rpy
|
||||
temp = self.state.imu_state.temperature
|
||||
return {'accel':accel, 'gyro':gyro, 'quat':quat, "rpy":rpy, 'temp':temp}
|
||||
|
||||
def getFootContacts(self):
|
||||
"""Returns the raw foot contact forces"""
|
||||
footContacts = self.state.foot_force
|
||||
return np.array(footContacts)
|
||||
|
||||
def getJointStates(self):
|
||||
"""Returns the joint angles (q) and velocities (dq) of the robot"""
|
||||
if self.state is None:
|
||||
return None
|
||||
motorStates = self.state.motor_state
|
||||
_q, _dq = zip(
|
||||
*[(motorState.q, motorState.dq) for motorState in motorStates[:12]]
|
||||
)
|
||||
q, dq = np.array(_q), np.array(_dq)
|
||||
|
||||
return {'q':q, 'dq':dq}
|
||||
|
||||
def getRemoteState(self):
|
||||
"""A method to get the state of the wireless remote control.
|
||||
Returns a xRockerBtn object:
|
||||
- head: [head1, head2]
|
||||
- keySwitch: xKeySwitch object
|
||||
- lx: float
|
||||
- rx: float
|
||||
- ry: float
|
||||
- L2: float
|
||||
- ly: float
|
||||
"""
|
||||
wirelessRemote = self.state.wireless_remote[:24]
|
||||
|
||||
binary_data = bytes(wirelessRemote)
|
||||
|
||||
format_str = "<2BH5f"
|
||||
data = struct.unpack(format_str, binary_data)
|
||||
|
||||
head = list(data[:2])
|
||||
lx = data[3]
|
||||
rx = data[4]
|
||||
ry = data[5]
|
||||
L2 = data[6]
|
||||
ly = data[7]
|
||||
|
||||
_btn = bin(data[2])[2:].zfill(16)
|
||||
btn = [int(char) for char in _btn]
|
||||
btn.reverse()
|
||||
|
||||
keySwitch = xKeySwitch(*btn)
|
||||
rockerBtn = xRockerBtn(head, keySwitch, lx, rx, ry, L2, ly)
|
||||
return rockerBtn
|
||||
|
||||
def getCommandFromRemote(self):
|
||||
"""Do not use directly for control!!!"""
|
||||
rockerBtn = self.getRemoteState()
|
||||
|
||||
lx = rockerBtn.lx
|
||||
ly = rockerBtn.ly
|
||||
rx = rockerBtn.rx
|
||||
|
||||
v_x = ly * self.vx_max
|
||||
v_y = lx * self.vy_max
|
||||
ω = rx * self.ωz_max
|
||||
|
||||
return v_x, v_y, ω
|
||||
|
||||
def getBatteryState(self):
|
||||
"""Returns the battery percentage of the robot"""
|
||||
batteryState = self.state.bms
|
||||
return batteryState.SOC
|
||||
|
||||
def setCommandsHigh(self, v_x, v_y, ω_z, bodyHeight=0.0, footRaiseHeight=0.0, mode=2):
|
||||
self.cmd_watchdog_timer = time.time()
|
||||
_v_x, _v_y, _ω_z = self.clip_velocity(v_x, v_y, ω_z)
|
||||
self.highcmd.header.stamp = self.get_clock().now().to_msg()
|
||||
self.highcmd.header.frame_id = "base_link"
|
||||
self.highcmd.twist.linear.x = _v_x
|
||||
self.highcmd.twist.linear.y = _v_y
|
||||
self.highcmd.twist.angular.z = _ω_z
|
||||
self.highcmd_publisher.publish(self.highcmd)
|
||||
|
||||
def setCommandsLow(self, q_des, dq_des, kp, kd, tau_ff):
|
||||
assert q_des.size == dq_des.size == kp.size == kd.size == tau_ff.size == 12, "q, dq, kp, kd, tau_ff should have size 12"
|
||||
lowcmd = Go2pyLowCmd()
|
||||
lowcmd.q = q_des.tolist()
|
||||
lowcmd.dq = dq_des.tolist()
|
||||
lowcmd.kp = kp.tolist()
|
||||
lowcmd.kd = kd.tolist()
|
||||
lowcmd.tau = tau_ff.tolist()
|
||||
self.lowcmd_publisher.publish(lowcmd)
|
||||
self.latest_command_stamp = time.time()
|
||||
|
||||
|
||||
def close(self):
|
||||
self.running = False
|
||||
self.thread.join()
|
||||
self.destroy_node()
|
||||
|
||||
def check_calf_collision(self, q):
|
||||
self.pin_robot.update(q)
|
||||
in_collision = self.pin_robot.check_calf_collision(q)
|
||||
return in_collision
|
||||
|
||||
def clip_velocity(self, v_x, v_y, ω_z):
|
||||
_v = np.array([[v_x], [v_y]])
|
||||
_scale = np.sqrt(_v.T @ self.P_v_max @ _v)[0, 0]
|
||||
|
||||
if _scale > 1.0:
|
||||
scale = 1.0 / _scale
|
||||
else:
|
||||
scale = 1.0
|
||||
|
||||
return scale * v_x, scale * v_y, np.clip(ω_z, self.ωz_min, self.ωz_max)
|
||||
|
||||
def overheat(self):
|
||||
return False
|
||||
|
||||
def getGravityInBody(self):
|
||||
q = self.getIMU()['quat']
|
||||
R = Rotation.from_quat([q[1], q[2], q[3], q[0]]).as_matrix()
|
||||
g_in_body = R.T@np.array([0.0, 0.0, -1.0]).reshape(3, 1)
|
||||
return g_in_body
|
||||
|
|
@ -178,12 +178,12 @@ class Go2Model:
|
|||
q (np.ndarray): A numpy array of size 19 representing the [x, y, z, qx, qy, qz, qw] and joint configurations in FR, FL, RR, RL order.
|
||||
dq (np.ndarray): A numpy array of size 18 representing the [vx, vy, vz, wx, wy, wz] and joint configurations in FR, FL, RR, RL order.
|
||||
"""
|
||||
self.robot.centroidalMomentum(q_,dq_)
|
||||
self.nle_ = self.robot.nle(q_, dq_)[self.dq_reordering_idx]
|
||||
self.g_ = self.robot.gravity(q_)[self.dq_reordering_idx]
|
||||
self.M_ = self.robot.mass(q_)[self.dq_reordering_idx,:]
|
||||
self.robot.centroidalMomentum(q,dq)
|
||||
self.nle_ = self.robot.nle(q, dq)[self.dq_reordering_idx]
|
||||
self.g_ = self.robot.gravity(q)[self.dq_reordering_idx]
|
||||
self.M_ = self.robot.mass(q)[self.dq_reordering_idx,:]
|
||||
self.M_ = self.M_[:,self.dq_reordering_idx]
|
||||
self.Minv_ = pin.computeMinverse(self.robot.model, self.robot.data, q_)[self.dq_reordering_idx,:]
|
||||
self.Minv_ = pin.computeMinverse(self.robot.model, self.robot.data, q)[self.dq_reordering_idx,:]
|
||||
self.Minv_ = self.Minv_[:,self.dq_reordering_idx]
|
||||
|
||||
|
||||
|
|
@ -231,7 +231,7 @@ class Go2Model:
|
|||
|
||||
def getGroundReactionForce(self, tau_est, body_acceleration=None):
|
||||
if body_acceleration is None:
|
||||
grf = {key:np.linalg.pinv(self.ef_J_[key][:3,6:].T)@(tau_est.squeeze() - self.nle_[6:]) for key in self.ef_J_.keys()}
|
||||
grf = {key:np.linalg.pinv(self.ef_Jw_[key][:3,6:].T)@(tau_est.squeeze() - self.nle_[6:]) for key in self.ef_Jw_.keys()}
|
||||
else:
|
||||
raise NotImplementedError("Ground reaction force with body dynamics is not implemented")
|
||||
return grf
|
||||
|
|
@ -1,19 +0,0 @@
|
|||
BmsState_
|
||||
msg
|
||||
|
||||
|
||||
Go2pyLowCmd
|
||||
msg
|
||||
|
||||
|
||||
IMUState_
|
||||
msg
|
||||
|
||||
|
||||
LowState_
|
||||
msg
|
||||
|
||||
|
||||
MotorState_
|
||||
msg
|
||||
|
||||
|
|
@ -1,19 +0,0 @@
|
|||
BmsState_
|
||||
dds_
|
||||
|
||||
|
||||
Go2pyLowCmd
|
||||
dds_
|
||||
|
||||
|
||||
IMUState_
|
||||
dds_
|
||||
|
||||
|
||||
LowState_
|
||||
dds_
|
||||
|
||||
|
||||
MotorState_
|
||||
dds_
|
||||
|
||||
|
|
@ -1,19 +0,0 @@
|
|||
BmsState_
|
||||
|
||||
BmsState_
|
||||
|
||||
Go2pyLowCmd
|
||||
|
||||
Go2pyLowCmd_
|
||||
|
||||
IMUState_
|
||||
|
||||
IMUState_
|
||||
|
||||
LowState_
|
||||
|
||||
LowState_
|
||||
|
||||
MotorState_
|
||||
|
||||
MotorState_
|
||||
|
|
@ -1,35 +0,0 @@
|
|||
"""
|
||||
Generated by Eclipse Cyclone DDS idlc Python Backend
|
||||
Cyclone DDS IDL version: v0.11.0
|
||||
Module: Go2Py.unitree_go.msg.dds_
|
||||
IDL file: BmsState_.idl
|
||||
|
||||
"""
|
||||
|
||||
from enum import auto
|
||||
from typing import TYPE_CHECKING, Optional
|
||||
from dataclasses import dataclass
|
||||
|
||||
import cyclonedds.idl as idl
|
||||
import cyclonedds.idl.annotations as annotate
|
||||
import cyclonedds.idl.types as types
|
||||
|
||||
# root module import for resolving types
|
||||
import Go2Py.unitree_go
|
||||
|
||||
|
||||
@dataclass
|
||||
@annotate.final
|
||||
@annotate.autoid("sequential")
|
||||
class BmsState_(idl.IdlStruct, typename="Go2Py.unitree_go.msg.dds_.BmsState_"):
|
||||
version_high: types.uint8
|
||||
version_low: types.uint8
|
||||
status: types.uint8
|
||||
soc: types.uint8
|
||||
current: types.int32
|
||||
cycle: types.uint16
|
||||
bq_ntc: types.array[types.uint8, 2]
|
||||
mcu_ntc: types.array[types.uint8, 2]
|
||||
cell_vol: types.array[types.uint16, 15]
|
||||
|
||||
|
||||
|
|
@ -1,48 +0,0 @@
|
|||
"""
|
||||
Generated by Eclipse Cyclone DDS idlc Python Backend
|
||||
Cyclone DDS IDL version: v0.11.0
|
||||
Module: Go2Py.unitree_go.msg.dds_
|
||||
IDL file: LowState_.idl
|
||||
|
||||
"""
|
||||
|
||||
from enum import auto
|
||||
from typing import TYPE_CHECKING, Optional
|
||||
from dataclasses import dataclass
|
||||
|
||||
import cyclonedds.idl as idl
|
||||
import cyclonedds.idl.annotations as annotate
|
||||
import cyclonedds.idl.types as types
|
||||
|
||||
# root module import for resolving types
|
||||
import Go2Py.unitree_go
|
||||
|
||||
|
||||
@dataclass
|
||||
@annotate.final
|
||||
@annotate.autoid("sequential")
|
||||
class LowState_(idl.IdlStruct, typename="Go2Py.unitree_go.msg.dds_.LowState_"):
|
||||
head: types.array[types.uint8, 2]
|
||||
level_flag: types.uint8
|
||||
frame_reserve: types.uint8
|
||||
sn: types.array[types.uint32, 2]
|
||||
version: types.array[types.uint32, 2]
|
||||
bandwidth: types.uint16
|
||||
imu_state: 'Go2Py.unitree_go.msg.dds_.IMUState_'
|
||||
motor_state: types.array['Go2Py.unitree_go.msg.dds_.MotorState_', 20]
|
||||
bms_state: 'Go2Py.unitree_go.msg.dds_.BmsState_'
|
||||
foot_force: types.array[types.int16, 4]
|
||||
foot_force_est: types.array[types.int16, 4]
|
||||
tick: types.uint32
|
||||
wireless_remote: types.array[types.uint8, 40]
|
||||
bit_flag: types.uint8
|
||||
adc_reel: types.float32
|
||||
temperature_ntc1: types.uint8
|
||||
temperature_ntc2: types.uint8
|
||||
power_v: types.float32
|
||||
power_a: types.float32
|
||||
fan_frequency: types.array[types.uint16, 4]
|
||||
reserve: types.uint32
|
||||
crc: types.uint32
|
||||
|
||||
|
||||
|
|
@ -1,37 +0,0 @@
|
|||
"""
|
||||
Generated by Eclipse Cyclone DDS idlc Python Backend
|
||||
Cyclone DDS IDL version: v0.11.0
|
||||
Module: Go2Py.unitree_go.msg.dds_
|
||||
IDL file: MotorState_.idl
|
||||
|
||||
"""
|
||||
|
||||
from enum import auto
|
||||
from typing import TYPE_CHECKING, Optional
|
||||
from dataclasses import dataclass
|
||||
|
||||
import cyclonedds.idl as idl
|
||||
import cyclonedds.idl.annotations as annotate
|
||||
import cyclonedds.idl.types as types
|
||||
|
||||
# root module import for resolving types
|
||||
import Go2Py.unitree_go
|
||||
|
||||
|
||||
@dataclass
|
||||
@annotate.final
|
||||
@annotate.autoid("sequential")
|
||||
class MotorState_(idl.IdlStruct, typename="Go2Py.unitree_go.msg.dds_.MotorState_"):
|
||||
mode: types.uint8
|
||||
q: types.float32
|
||||
dq: types.float32
|
||||
ddq: types.float32
|
||||
tau_est: types.float32
|
||||
q_raw: types.float32
|
||||
dq_raw: types.float32
|
||||
ddq_raw: types.float32
|
||||
temperature: types.uint8
|
||||
lost: types.uint32
|
||||
reserve: types.array[types.uint32, 2]
|
||||
|
||||
|
||||
|
|
@ -1,13 +0,0 @@
|
|||
"""
|
||||
Generated by Eclipse Cyclone DDS idlc Python Backend
|
||||
Cyclone DDS IDL version: v0.11.0
|
||||
Module: Go2Py.unitree_go.msg.dds_
|
||||
|
||||
"""
|
||||
|
||||
from ._BmsState_ import BmsState_
|
||||
from ._Go2pyLowCmd import Go2pyLowCmd_
|
||||
from ._IMUState_ import IMUState_
|
||||
from ._LowState_ import LowState_
|
||||
from ._MotorState_ import MotorState_
|
||||
__all__ = ["BmsState_", "Go2pyLowCmd_", "IMUState_", "LowState_", "MotorState_", ]
|
||||
61
Makefile
61
Makefile
|
|
@ -1,29 +1,44 @@
|
|||
docker_dock:
|
||||
@docker build --no-cache --tag go2py_dock_hw:latest -f Dockerfile.dock .
|
||||
messages:
|
||||
@cd scripts && ./make_msgs.sh
|
||||
|
||||
docker_robot:
|
||||
@docker build --no-cache --tag go2py_robot_hw:latest -f Dockerfile.robot .
|
||||
realsense:
|
||||
@cd deploy/docker && docker build --tag go2py_realsense:latest -f Dockerfile.realsense .
|
||||
|
||||
docker_nav:
|
||||
# @docker build --no-cache --tag go2py_nav:latest -f Dockerfile.nav2 .
|
||||
@docker build --tag go2py_nav:latest -f Dockerfile.nav2 .
|
||||
hesai:
|
||||
@cd deploy && docker build --no-cache --tag go2py_hesai:latest -f docker/Dockerfile.hesai .
|
||||
|
||||
docker_dock_install:
|
||||
@cp deploy/scripts/go2py-dock-hw-nodes.service /etc/systemd/system/
|
||||
@systemctl enable go2py-dock-hw-nodes.service
|
||||
@systemctl start go2py-dock-hw-nodes.service
|
||||
bridge:
|
||||
@cd deploy && docker build --no-cache --tag go2py_bridge:latest -f docker/Dockerfile.bridge .
|
||||
|
||||
docker_dock_uninstall:
|
||||
@systemctl disable go2py-dock-hw-nodes.service
|
||||
@systemctl stop go2py-dock-hw-nodes.service
|
||||
@rm /etc/systemd/system/go2py-dock-hw-nodes.service
|
||||
robot_description:
|
||||
@cd deploy && docker build --no-cache --tag go2py_description:latest -f docker/Dockerfile.robot_description .
|
||||
|
||||
docker_robot_install:
|
||||
@cp deploy/scripts/go2py-robot-hw-nodes.service /etc/systemd/system/
|
||||
@systemctl enable go2py-robot-hw-nodes.service
|
||||
@systemctl start go2py-robot-hw-nodes.service
|
||||
hesai_install:
|
||||
@cp deploy/services/go2py-hesai.service /etc/systemd/system/
|
||||
@systemctl enable go2py-hesai.service
|
||||
@systemctl start go2py-hesai.service
|
||||
|
||||
docker_robot_uninstall:
|
||||
@systemctl disable go2py-robot-hw-nodes.service
|
||||
@systemctl stop go2py-robot-hw-nodes.service
|
||||
@rm /etc/systemd/system/go2py-robot-hw-nodes.service
|
||||
bridge_install:
|
||||
@cp deploy/services/go2py-bridge.service /etc/systemd/system/
|
||||
@systemctl enable go2py-bridge.service
|
||||
@systemctl start go2py-bridge.service
|
||||
|
||||
robot_description_install:
|
||||
@cp deploy/services/go2py-robot-description.service /etc/systemd/system/
|
||||
@systemctl enable go2py-robot-description.service
|
||||
@systemctl start go2py-robot-description.service
|
||||
|
||||
hesai_uninstall:
|
||||
@systemctl disable go2py-hesai.service
|
||||
@systemctl stop go2py-hesai.service
|
||||
@rm /etc/systemd/system/go2py-hesai.service
|
||||
|
||||
bridge_uninstall:
|
||||
@systemctl disable go2py-bridge.service
|
||||
@systemctl stop go2py-bridge.service
|
||||
@rm /etc/systemd/system/go2py-bridge.service
|
||||
|
||||
robot_description_uninstall:
|
||||
@systemctl disable go2py-robot-description.service
|
||||
@systemctl stop go2py-robot-description.service
|
||||
@rm /etc/systemd/system/go2py-robot-description.service
|
||||
26
README.md
26
README.md
|
|
@ -1,13 +1,13 @@
|
|||
# Go2Py
|
||||
|
||||
Go2Py is a Pythonic interface and driver for low-level and high-level control of Unitree Go2 quadruped robots. The motivation of this project is to remove the burden of developing interface, safety systems, and basic components required for starting locomotion reserach using the Go2 quadruped robot. It provides a modular pipeline for real-time communication with the robot in both simulated and real world environment with a unified interface.
|
||||
Go2Py is a Pythonic interface and driver for low-level and high-level control of Unitree Go2 quadruped robots. The motivation of this project is to remove the burden of developing interface, safety systems, and basic components required for starting locomotion research using the Go2 quadruped robot. It provides a modular pipeline for real-time communication with the robot in both simulated and real-world environments with a unified interface.
|
||||
|
||||
<p align="center">
|
||||
<img src="docs/assets/openfig.png" alt="image" width="60%" height="auto"/>
|
||||
</p>
|
||||
|
||||
This project is comprised of the following components:
|
||||
- **C++ Bridge:** A dockerized ROS2 bridge built upon the [unitree_ros2](https://github.com/unitreerobotics/unitree_ros2) that implements a remote controlled emergency stop and publishes the robot states as standard ROS2 topics usable by upstream systems such as NAV2.
|
||||
- **C++ Bridge:** A dockerized ROS2 bridge built upon the [unitree_ros2](https://github.com/unitreerobotics/unitree_ros2) that implements a remote-controlled emergency stop and publishes the robot states as standard ROS2 topics usable by upstream systems such as NAV2.
|
||||
- **Robot Interface:** A simple Python class that represents the robot and communicates with the C++ bridge through either DDS (ROS independent) or ROS2 interfaces.
|
||||
- **Robot Management FSM:** A finite state machine for controlling the behavior of the robot up to the point of handover to the user low-level controller (sitting down, standing up) with safety monitors (motor temperatures, emergency stops).
|
||||
- **Robot Model:** A simple to use [Pinocchio](https://github.com/stack-of-tasks/pinocchio) wrapper for computing the kinematics and dynamics parameters of the robot.
|
||||
|
|
@ -20,7 +20,6 @@ from Go2Py.robot.interface.dds import GO2Real
|
|||
from Go2Py.robot.model import Go2Model
|
||||
robot = GO2Real(mode='lowlevel')
|
||||
model = Go2Model()
|
||||
robot.standDownReset()
|
||||
while running:
|
||||
joint_state = robot.getJointStates()
|
||||
imu = robot.getIMU()
|
||||
|
|
@ -32,7 +31,7 @@ while running:
|
|||
|
||||
robot.setCommands(q_des, dq_des, kp, kd, tau_ff)
|
||||
```
|
||||
An identical workflow is can be followed for simulation:
|
||||
An identical workflow can be followed for simulation:
|
||||
```python
|
||||
from Go2Py.sim.mujoco import Go2Sim
|
||||
from Go2Py.robot.model import Go2Model
|
||||
|
|
@ -52,17 +51,14 @@ while running:
|
|||
robot.step()
|
||||
```
|
||||
## Installation
|
||||
Follow through the steps in here to [setup](docs/setup.md) the robot and Go2Py.
|
||||
Follow through the steps here to [setup](docs/setup.md) the robot and Go2Py.
|
||||
|
||||
## Further Examples
|
||||
A set of sorted examples are provided in the [examples](examples) directory to get you up and running quickly:
|
||||
A set of examples and tutorials are provided to get you up and running quickly:
|
||||
|
||||
- High-level body velocity interface (ROS2)
|
||||
- High-level body velocity interface (DDS)
|
||||
- Low-level joint interface (ROS2)
|
||||
- Low-level joint interface (DDS)
|
||||
- Low-level simulation interface
|
||||
- Contact Force Estimation
|
||||
- Foot Contact Estimation
|
||||
- Extended Kalman Filter Legged Inertial State Estimator
|
||||
- Walk These Ways RL Controller
|
||||
- [Communicating with the robot](examples/00-robot-interface.ipynb)
|
||||
- [Interracting with ROS2](examples/01-ros2-tools.ipynb)
|
||||
- [Simulating with MuJoCo](examples/02-MuJoCo-sim.ipynb)
|
||||
- [Robot dynamics and kinematics](examples/03-robot-dynamic-model.ipynb)
|
||||
- [Finite state machine in low-level control](examples/04-FSM.ipynb)
|
||||
- [Walk These Ways RL Controller](examples/05-walk-these-ways-RL-controller.ipynb)
|
||||
|
|
|
|||
|
|
@ -1,11 +0,0 @@
|
|||
project(go2py_bridge)
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
|
||||
SET(CMAKE_CXX_STANDARD 17)
|
||||
add_library(msgs STATIC include/go2py/LowCmd.cpp include/go2py/LowState.cpp)
|
||||
|
||||
include_directories(/usr/local/include/ddscxx /usr/local/include/iceoryx/v2.0.2 include)
|
||||
link_libraries(unitree_sdk2 ddsc ddscxx rt pthread msgs)
|
||||
|
||||
add_executable(lowlevel_bridge src/lowlevel/bridge.cpp)
|
||||
add_executable(simple_test src/simple_test.cpp)
|
||||
|
|
@ -1,9 +0,0 @@
|
|||
module msgs{
|
||||
struct Imu {
|
||||
float q[4];
|
||||
float gyro[3];
|
||||
float accel[3];
|
||||
float rpy[3];
|
||||
octet tmp;
|
||||
};
|
||||
};
|
||||
|
|
@ -1,10 +0,0 @@
|
|||
module msgs{
|
||||
struct LowCmd {
|
||||
float q[12];
|
||||
float dq[12];
|
||||
float tau_ff[12];
|
||||
float kp[12];
|
||||
float kv[12];
|
||||
octet e_stop;
|
||||
};
|
||||
};
|
||||
|
|
@ -1,17 +0,0 @@
|
|||
module msgs{
|
||||
struct LowState {
|
||||
float q[12];
|
||||
float dq[12];
|
||||
float ddq[12];
|
||||
float tau_est[12];
|
||||
float tmp[12];
|
||||
float contact[4];
|
||||
float quat[4];
|
||||
float gyro[3];
|
||||
float accel[3];
|
||||
float rpy[3];
|
||||
octet imu_tmp;
|
||||
float voltage;
|
||||
float current;
|
||||
};
|
||||
};
|
||||
|
|
@ -1,57 +0,0 @@
|
|||
/****************************************************************
|
||||
|
||||
Generated by Eclipse Cyclone DDS IDL to CXX Translator
|
||||
File name: Imu.idl
|
||||
Source: Imu.cpp
|
||||
Cyclone DDS: v0.10.2
|
||||
|
||||
*****************************************************************/
|
||||
#include "Imu.hpp"
|
||||
|
||||
namespace org{
|
||||
namespace eclipse{
|
||||
namespace cyclonedds{
|
||||
namespace core{
|
||||
namespace cdr{
|
||||
|
||||
template<>
|
||||
propvec &get_type_props<::msgs::Imu>() {
|
||||
static thread_local std::mutex mtx;
|
||||
static thread_local propvec props;
|
||||
static thread_local entity_properties_t *props_end = nullptr;
|
||||
static thread_local std::atomic_bool initialized {false};
|
||||
key_endpoint keylist;
|
||||
if (initialized.load(std::memory_order_relaxed)) {
|
||||
auto ptr = props.data();
|
||||
while (ptr < props_end)
|
||||
(ptr++)->is_present = false;
|
||||
return props;
|
||||
}
|
||||
std::lock_guard<std::mutex> lock(mtx);
|
||||
if (initialized.load(std::memory_order_relaxed)) {
|
||||
auto ptr = props.data();
|
||||
while (ptr < props_end)
|
||||
(ptr++)->is_present = false;
|
||||
return props;
|
||||
}
|
||||
props.clear();
|
||||
|
||||
props.push_back(entity_properties_t(0, 0, false, bb_unset, extensibility::ext_final)); //root
|
||||
props.push_back(entity_properties_t(1, 0, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::q
|
||||
props.push_back(entity_properties_t(1, 1, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::gyro
|
||||
props.push_back(entity_properties_t(1, 2, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::accel
|
||||
props.push_back(entity_properties_t(1, 3, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::rpy
|
||||
props.push_back(entity_properties_t(1, 4, false, get_bit_bound<uint8_t>(), extensibility::ext_final, false)); //::tmp
|
||||
|
||||
entity_properties_t::finish(props, keylist);
|
||||
props_end = props.data() + props.size();
|
||||
initialized.store(true, std::memory_order_release);
|
||||
return props;
|
||||
}
|
||||
|
||||
} //namespace cdr
|
||||
} //namespace core
|
||||
} //namespace cyclonedds
|
||||
} //namespace eclipse
|
||||
} //namespace org
|
||||
|
||||
|
|
@ -1,491 +0,0 @@
|
|||
/****************************************************************
|
||||
|
||||
Generated by Eclipse Cyclone DDS IDL to CXX Translator
|
||||
File name: Imu.idl
|
||||
Source: Imu.hpp
|
||||
Cyclone DDS: v0.10.2
|
||||
|
||||
*****************************************************************/
|
||||
#ifndef DDSCXX_IMU_HPP
|
||||
#define DDSCXX_IMU_HPP
|
||||
|
||||
#include <cstdint>
|
||||
#include <array>
|
||||
|
||||
namespace msgs
|
||||
{
|
||||
class Imu
|
||||
{
|
||||
private:
|
||||
std::array<float, 4> q_ = { };
|
||||
std::array<float, 3> gyro_ = { };
|
||||
std::array<float, 3> accel_ = { };
|
||||
std::array<float, 3> rpy_ = { };
|
||||
uint8_t tmp_ = 0;
|
||||
|
||||
public:
|
||||
Imu() = default;
|
||||
|
||||
explicit Imu(
|
||||
const std::array<float, 4>& q,
|
||||
const std::array<float, 3>& gyro,
|
||||
const std::array<float, 3>& accel,
|
||||
const std::array<float, 3>& rpy,
|
||||
uint8_t tmp) :
|
||||
q_(q),
|
||||
gyro_(gyro),
|
||||
accel_(accel),
|
||||
rpy_(rpy),
|
||||
tmp_(tmp) { }
|
||||
|
||||
const std::array<float, 4>& q() const { return this->q_; }
|
||||
std::array<float, 4>& q() { return this->q_; }
|
||||
void q(const std::array<float, 4>& _val_) { this->q_ = _val_; }
|
||||
void q(std::array<float, 4>&& _val_) { this->q_ = _val_; }
|
||||
const std::array<float, 3>& gyro() const { return this->gyro_; }
|
||||
std::array<float, 3>& gyro() { return this->gyro_; }
|
||||
void gyro(const std::array<float, 3>& _val_) { this->gyro_ = _val_; }
|
||||
void gyro(std::array<float, 3>&& _val_) { this->gyro_ = _val_; }
|
||||
const std::array<float, 3>& accel() const { return this->accel_; }
|
||||
std::array<float, 3>& accel() { return this->accel_; }
|
||||
void accel(const std::array<float, 3>& _val_) { this->accel_ = _val_; }
|
||||
void accel(std::array<float, 3>&& _val_) { this->accel_ = _val_; }
|
||||
const std::array<float, 3>& rpy() const { return this->rpy_; }
|
||||
std::array<float, 3>& rpy() { return this->rpy_; }
|
||||
void rpy(const std::array<float, 3>& _val_) { this->rpy_ = _val_; }
|
||||
void rpy(std::array<float, 3>&& _val_) { this->rpy_ = _val_; }
|
||||
uint8_t tmp() const { return this->tmp_; }
|
||||
uint8_t& tmp() { return this->tmp_; }
|
||||
void tmp(uint8_t _val_) { this->tmp_ = _val_; }
|
||||
|
||||
bool operator==(const Imu& _other) const
|
||||
{
|
||||
(void) _other;
|
||||
return q_ == _other.q_ &&
|
||||
gyro_ == _other.gyro_ &&
|
||||
accel_ == _other.accel_ &&
|
||||
rpy_ == _other.rpy_ &&
|
||||
tmp_ == _other.tmp_;
|
||||
}
|
||||
|
||||
bool operator!=(const Imu& _other) const
|
||||
{
|
||||
return !(*this == _other);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#include "dds/topic/TopicTraits.hpp"
|
||||
#include "org/eclipse/cyclonedds/topic/datatopic.hpp"
|
||||
|
||||
namespace org {
|
||||
namespace eclipse {
|
||||
namespace cyclonedds {
|
||||
namespace topic {
|
||||
|
||||
template <> constexpr const char* TopicTraits<::msgs::Imu>::getTypeName()
|
||||
{
|
||||
return "msgs::Imu";
|
||||
}
|
||||
|
||||
template <> constexpr bool TopicTraits<::msgs::Imu>::isKeyless()
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
#ifdef DDSCXX_HAS_TYPE_DISCOVERY
|
||||
template<> constexpr unsigned int TopicTraits<::msgs::Imu>::type_map_blob_sz() { return 442; }
|
||||
template<> constexpr unsigned int TopicTraits<::msgs::Imu>::type_info_blob_sz() { return 100; }
|
||||
template<> inline const uint8_t * TopicTraits<::msgs::Imu>::type_map_blob() {
|
||||
static const uint8_t blob[] = {
|
||||
0xab, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0xf1, 0xab, 0x8a, 0x74, 0xa0, 0x59, 0x30, 0x63,
|
||||
0x7f, 0xb3, 0x36, 0xe0, 0x64, 0x6f, 0xd3, 0x00, 0x93, 0x00, 0x00, 0x00, 0xf1, 0x51, 0x01, 0x00,
|
||||
0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x83, 0x00, 0x00, 0x00, 0x05, 0x00, 0x00, 0x00,
|
||||
0x16, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00,
|
||||
0x01, 0x00, 0x00, 0x00, 0x04, 0x09, 0x76, 0x94, 0xf4, 0xa6, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00,
|
||||
0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00,
|
||||
0x03, 0x09, 0x41, 0xe1, 0xdb, 0x58, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00,
|
||||
0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x03, 0x09, 0x98, 0x30,
|
||||
0x99, 0x65, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3,
|
||||
0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x03, 0x09, 0xb0, 0x7d, 0x92, 0xed, 0x00, 0x00,
|
||||
0x0b, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x01, 0x00, 0x02, 0xfa, 0x81, 0x6e, 0xdb, 0x00,
|
||||
0xde, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0xf2, 0xa1, 0x90, 0x68, 0x0b, 0xcf, 0x24, 0x8c,
|
||||
0xb4, 0x1b, 0xaa, 0x07, 0x8f, 0xf4, 0x93, 0x00, 0xc6, 0x00, 0x00, 0x00, 0xf2, 0x51, 0x01, 0x00,
|
||||
0x12, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x6d, 0x73, 0x67, 0x73,
|
||||
0x3a, 0x3a, 0x49, 0x6d, 0x75, 0x00, 0x00, 0x00, 0xa6, 0x00, 0x00, 0x00, 0x05, 0x00, 0x00, 0x00,
|
||||
0x1c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00,
|
||||
0x01, 0x00, 0x00, 0x00, 0x04, 0x09, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x71, 0x00, 0x00, 0x00,
|
||||
0x1f, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00,
|
||||
0x01, 0x00, 0x00, 0x00, 0x03, 0x09, 0x00, 0x00, 0x05, 0x00, 0x00, 0x00, 0x67, 0x79, 0x72, 0x6f,
|
||||
0x00, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3,
|
||||
0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x03, 0x09, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00,
|
||||
0x61, 0x63, 0x63, 0x65, 0x6c, 0x00, 0x00, 0x00, 0x1e, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00,
|
||||
0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x03, 0x09, 0x00, 0x00,
|
||||
0x04, 0x00, 0x00, 0x00, 0x72, 0x70, 0x79, 0x00, 0x00, 0x00, 0x00, 0x00, 0x12, 0x00, 0x00, 0x00,
|
||||
0x04, 0x00, 0x00, 0x00, 0x01, 0x00, 0x02, 0x00, 0x04, 0x00, 0x00, 0x00, 0x74, 0x6d, 0x70, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x22, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0xf2, 0xa1, 0x90, 0x68,
|
||||
0x0b, 0xcf, 0x24, 0x8c, 0xb4, 0x1b, 0xaa, 0x07, 0x8f, 0xf4, 0x93, 0xf1, 0xab, 0x8a, 0x74, 0xa0,
|
||||
0x59, 0x30, 0x63, 0x7f, 0xb3, 0x36, 0xe0, 0x64, 0x6f, 0xd3, };
|
||||
return blob;
|
||||
}
|
||||
template<> inline const uint8_t * TopicTraits<::msgs::Imu>::type_info_blob() {
|
||||
static const uint8_t blob[] = {
|
||||
0x60, 0x00, 0x00, 0x00, 0x01, 0x10, 0x00, 0x40, 0x28, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00,
|
||||
0x14, 0x00, 0x00, 0x00, 0xf1, 0xab, 0x8a, 0x74, 0xa0, 0x59, 0x30, 0x63, 0x7f, 0xb3, 0x36, 0xe0,
|
||||
0x64, 0x6f, 0xd3, 0x00, 0x97, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x02, 0x10, 0x00, 0x40, 0x28, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00,
|
||||
0x14, 0x00, 0x00, 0x00, 0xf2, 0xa1, 0x90, 0x68, 0x0b, 0xcf, 0x24, 0x8c, 0xb4, 0x1b, 0xaa, 0x07,
|
||||
0x8f, 0xf4, 0x93, 0x00, 0xca, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, };
|
||||
return blob;
|
||||
}
|
||||
#endif //DDSCXX_HAS_TYPE_DISCOVERY
|
||||
|
||||
} //namespace topic
|
||||
} //namespace cyclonedds
|
||||
} //namespace eclipse
|
||||
} //namespace org
|
||||
|
||||
namespace dds {
|
||||
namespace topic {
|
||||
|
||||
template <>
|
||||
struct topic_type_name<::msgs::Imu>
|
||||
{
|
||||
static std::string value()
|
||||
{
|
||||
return org::eclipse::cyclonedds::topic::TopicTraits<::msgs::Imu>::getTypeName();
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
REGISTER_TOPIC_TYPE(::msgs::Imu)
|
||||
|
||||
namespace org{
|
||||
namespace eclipse{
|
||||
namespace cyclonedds{
|
||||
namespace core{
|
||||
namespace cdr{
|
||||
|
||||
template<>
|
||||
propvec &get_type_props<::msgs::Imu>();
|
||||
|
||||
template<typename T, std::enable_if_t<std::is_base_of<cdr_stream, T>::value, bool> = true >
|
||||
bool write(T& streamer, const ::msgs::Imu& instance, entity_properties_t *props) {
|
||||
(void)instance;
|
||||
if (!streamer.start_struct(*props))
|
||||
return false;
|
||||
auto prop = streamer.first_entity(props);
|
||||
while (prop) {
|
||||
switch (prop->m_id) {
|
||||
case 0:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!write(streamer, instance.q()[0], instance.q().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 1:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!write(streamer, instance.gyro()[0], instance.gyro().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 2:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!write(streamer, instance.accel()[0], instance.accel().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 3:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!write(streamer, instance.rpy()[0], instance.rpy().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 4:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!write(streamer, instance.tmp()))
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
prop = streamer.next_entity(prop);
|
||||
}
|
||||
return streamer.finish_struct(*props);
|
||||
}
|
||||
|
||||
template<typename S, std::enable_if_t<std::is_base_of<cdr_stream, S>::value, bool> = true >
|
||||
bool write(S& str, const ::msgs::Imu& instance, bool as_key) {
|
||||
auto &props = get_type_props<::msgs::Imu>();
|
||||
str.set_mode(cdr_stream::stream_mode::write, as_key);
|
||||
return write(str, instance, props.data());
|
||||
}
|
||||
|
||||
template<typename T, std::enable_if_t<std::is_base_of<cdr_stream, T>::value, bool> = true >
|
||||
bool read(T& streamer, ::msgs::Imu& instance, entity_properties_t *props) {
|
||||
(void)instance;
|
||||
if (!streamer.start_struct(*props))
|
||||
return false;
|
||||
auto prop = streamer.first_entity(props);
|
||||
while (prop) {
|
||||
switch (prop->m_id) {
|
||||
case 0:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!read(streamer, instance.q()[0], instance.q().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 1:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!read(streamer, instance.gyro()[0], instance.gyro().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 2:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!read(streamer, instance.accel()[0], instance.accel().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 3:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!read(streamer, instance.rpy()[0], instance.rpy().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 4:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!read(streamer, instance.tmp()))
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
prop = streamer.next_entity(prop);
|
||||
}
|
||||
return streamer.finish_struct(*props);
|
||||
}
|
||||
|
||||
template<typename S, std::enable_if_t<std::is_base_of<cdr_stream, S>::value, bool> = true >
|
||||
bool read(S& str, ::msgs::Imu& instance, bool as_key) {
|
||||
auto &props = get_type_props<::msgs::Imu>();
|
||||
str.set_mode(cdr_stream::stream_mode::read, as_key);
|
||||
return read(str, instance, props.data());
|
||||
}
|
||||
|
||||
template<typename T, std::enable_if_t<std::is_base_of<cdr_stream, T>::value, bool> = true >
|
||||
bool move(T& streamer, const ::msgs::Imu& instance, entity_properties_t *props) {
|
||||
(void)instance;
|
||||
if (!streamer.start_struct(*props))
|
||||
return false;
|
||||
auto prop = streamer.first_entity(props);
|
||||
while (prop) {
|
||||
switch (prop->m_id) {
|
||||
case 0:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!move(streamer, instance.q()[0], instance.q().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 1:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!move(streamer, instance.gyro()[0], instance.gyro().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 2:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!move(streamer, instance.accel()[0], instance.accel().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 3:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!move(streamer, instance.rpy()[0], instance.rpy().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 4:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!move(streamer, instance.tmp()))
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
prop = streamer.next_entity(prop);
|
||||
}
|
||||
return streamer.finish_struct(*props);
|
||||
}
|
||||
|
||||
template<typename S, std::enable_if_t<std::is_base_of<cdr_stream, S>::value, bool> = true >
|
||||
bool move(S& str, const ::msgs::Imu& instance, bool as_key) {
|
||||
auto &props = get_type_props<::msgs::Imu>();
|
||||
str.set_mode(cdr_stream::stream_mode::move, as_key);
|
||||
return move(str, instance, props.data());
|
||||
}
|
||||
|
||||
template<typename T, std::enable_if_t<std::is_base_of<cdr_stream, T>::value, bool> = true >
|
||||
bool max(T& streamer, const ::msgs::Imu& instance, entity_properties_t *props) {
|
||||
(void)instance;
|
||||
if (!streamer.start_struct(*props))
|
||||
return false;
|
||||
auto prop = streamer.first_entity(props);
|
||||
while (prop) {
|
||||
switch (prop->m_id) {
|
||||
case 0:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!max(streamer, instance.q()[0], instance.q().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 1:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!max(streamer, instance.gyro()[0], instance.gyro().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 2:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!max(streamer, instance.accel()[0], instance.accel().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 3:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!max(streamer, instance.rpy()[0], instance.rpy().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 4:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!max(streamer, instance.tmp()))
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
prop = streamer.next_entity(prop);
|
||||
}
|
||||
return streamer.finish_struct(*props);
|
||||
}
|
||||
|
||||
template<typename S, std::enable_if_t<std::is_base_of<cdr_stream, S>::value, bool> = true >
|
||||
bool max(S& str, const ::msgs::Imu& instance, bool as_key) {
|
||||
auto &props = get_type_props<::msgs::Imu>();
|
||||
str.set_mode(cdr_stream::stream_mode::max, as_key);
|
||||
return max(str, instance, props.data());
|
||||
}
|
||||
|
||||
} //namespace cdr
|
||||
} //namespace core
|
||||
} //namespace cyclonedds
|
||||
} //namespace eclipse
|
||||
} //namespace org
|
||||
|
||||
#endif // DDSCXX_IMU_HPP
|
||||
|
|
@ -1,58 +0,0 @@
|
|||
/****************************************************************
|
||||
|
||||
Generated by Eclipse Cyclone DDS IDL to CXX Translator
|
||||
File name: LowCmd.idl
|
||||
Source: LowCmd.cpp
|
||||
Cyclone DDS: v0.10.2
|
||||
|
||||
*****************************************************************/
|
||||
#include "LowCmd.hpp"
|
||||
|
||||
namespace org{
|
||||
namespace eclipse{
|
||||
namespace cyclonedds{
|
||||
namespace core{
|
||||
namespace cdr{
|
||||
|
||||
template<>
|
||||
propvec &get_type_props<::msgs::LowCmd>() {
|
||||
static thread_local std::mutex mtx;
|
||||
static thread_local propvec props;
|
||||
static thread_local entity_properties_t *props_end = nullptr;
|
||||
static thread_local std::atomic_bool initialized {false};
|
||||
key_endpoint keylist;
|
||||
if (initialized.load(std::memory_order_relaxed)) {
|
||||
auto ptr = props.data();
|
||||
while (ptr < props_end)
|
||||
(ptr++)->is_present = false;
|
||||
return props;
|
||||
}
|
||||
std::lock_guard<std::mutex> lock(mtx);
|
||||
if (initialized.load(std::memory_order_relaxed)) {
|
||||
auto ptr = props.data();
|
||||
while (ptr < props_end)
|
||||
(ptr++)->is_present = false;
|
||||
return props;
|
||||
}
|
||||
props.clear();
|
||||
|
||||
props.push_back(entity_properties_t(0, 0, false, bb_unset, extensibility::ext_final)); //root
|
||||
props.push_back(entity_properties_t(1, 0, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::q
|
||||
props.push_back(entity_properties_t(1, 1, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::dq
|
||||
props.push_back(entity_properties_t(1, 2, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::tau_ff
|
||||
props.push_back(entity_properties_t(1, 3, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::kp
|
||||
props.push_back(entity_properties_t(1, 4, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::kv
|
||||
props.push_back(entity_properties_t(1, 5, false, get_bit_bound<uint8_t>(), extensibility::ext_final, false)); //::e_stop
|
||||
|
||||
entity_properties_t::finish(props, keylist);
|
||||
props_end = props.data() + props.size();
|
||||
initialized.store(true, std::memory_order_release);
|
||||
return props;
|
||||
}
|
||||
|
||||
} //namespace cdr
|
||||
} //namespace core
|
||||
} //namespace cyclonedds
|
||||
} //namespace eclipse
|
||||
} //namespace org
|
||||
|
||||
|
|
@ -1,552 +0,0 @@
|
|||
/****************************************************************
|
||||
|
||||
Generated by Eclipse Cyclone DDS IDL to CXX Translator
|
||||
File name: LowCmd.idl
|
||||
Source: LowCmd.hpp
|
||||
Cyclone DDS: v0.10.2
|
||||
|
||||
*****************************************************************/
|
||||
#ifndef DDSCXX_LOWCMD_HPP
|
||||
#define DDSCXX_LOWCMD_HPP
|
||||
|
||||
#include <cstdint>
|
||||
#include <array>
|
||||
|
||||
namespace msgs
|
||||
{
|
||||
class LowCmd
|
||||
{
|
||||
private:
|
||||
std::array<float, 12> q_ = { };
|
||||
std::array<float, 12> dq_ = { };
|
||||
std::array<float, 12> tau_ff_ = { };
|
||||
std::array<float, 12> kp_ = { };
|
||||
std::array<float, 12> kv_ = { };
|
||||
uint8_t e_stop_ = 0;
|
||||
|
||||
public:
|
||||
LowCmd() = default;
|
||||
|
||||
explicit LowCmd(
|
||||
const std::array<float, 12>& q,
|
||||
const std::array<float, 12>& dq,
|
||||
const std::array<float, 12>& tau_ff,
|
||||
const std::array<float, 12>& kp,
|
||||
const std::array<float, 12>& kv,
|
||||
uint8_t e_stop) :
|
||||
q_(q),
|
||||
dq_(dq),
|
||||
tau_ff_(tau_ff),
|
||||
kp_(kp),
|
||||
kv_(kv),
|
||||
e_stop_(e_stop) { }
|
||||
|
||||
const std::array<float, 12>& q() const { return this->q_; }
|
||||
std::array<float, 12>& q() { return this->q_; }
|
||||
void q(const std::array<float, 12>& _val_) { this->q_ = _val_; }
|
||||
void q(std::array<float, 12>&& _val_) { this->q_ = _val_; }
|
||||
const std::array<float, 12>& dq() const { return this->dq_; }
|
||||
std::array<float, 12>& dq() { return this->dq_; }
|
||||
void dq(const std::array<float, 12>& _val_) { this->dq_ = _val_; }
|
||||
void dq(std::array<float, 12>&& _val_) { this->dq_ = _val_; }
|
||||
const std::array<float, 12>& tau_ff() const { return this->tau_ff_; }
|
||||
std::array<float, 12>& tau_ff() { return this->tau_ff_; }
|
||||
void tau_ff(const std::array<float, 12>& _val_) { this->tau_ff_ = _val_; }
|
||||
void tau_ff(std::array<float, 12>&& _val_) { this->tau_ff_ = _val_; }
|
||||
const std::array<float, 12>& kp() const { return this->kp_; }
|
||||
std::array<float, 12>& kp() { return this->kp_; }
|
||||
void kp(const std::array<float, 12>& _val_) { this->kp_ = _val_; }
|
||||
void kp(std::array<float, 12>&& _val_) { this->kp_ = _val_; }
|
||||
const std::array<float, 12>& kv() const { return this->kv_; }
|
||||
std::array<float, 12>& kv() { return this->kv_; }
|
||||
void kv(const std::array<float, 12>& _val_) { this->kv_ = _val_; }
|
||||
void kv(std::array<float, 12>&& _val_) { this->kv_ = _val_; }
|
||||
uint8_t e_stop() const { return this->e_stop_; }
|
||||
uint8_t& e_stop() { return this->e_stop_; }
|
||||
void e_stop(uint8_t _val_) { this->e_stop_ = _val_; }
|
||||
|
||||
bool operator==(const LowCmd& _other) const
|
||||
{
|
||||
(void) _other;
|
||||
return q_ == _other.q_ &&
|
||||
dq_ == _other.dq_ &&
|
||||
tau_ff_ == _other.tau_ff_ &&
|
||||
kp_ == _other.kp_ &&
|
||||
kv_ == _other.kv_ &&
|
||||
e_stop_ == _other.e_stop_;
|
||||
}
|
||||
|
||||
bool operator!=(const LowCmd& _other) const
|
||||
{
|
||||
return !(*this == _other);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#include "dds/topic/TopicTraits.hpp"
|
||||
#include "org/eclipse/cyclonedds/topic/datatopic.hpp"
|
||||
|
||||
namespace org {
|
||||
namespace eclipse {
|
||||
namespace cyclonedds {
|
||||
namespace topic {
|
||||
|
||||
template <> constexpr const char* TopicTraits<::msgs::LowCmd>::getTypeName()
|
||||
{
|
||||
return "msgs::LowCmd";
|
||||
}
|
||||
|
||||
template <> constexpr bool TopicTraits<::msgs::LowCmd>::isKeyless()
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
#ifdef DDSCXX_HAS_TYPE_DISCOVERY
|
||||
template<> constexpr unsigned int TopicTraits<::msgs::LowCmd>::type_map_blob_sz() { return 518; }
|
||||
template<> constexpr unsigned int TopicTraits<::msgs::LowCmd>::type_info_blob_sz() { return 100; }
|
||||
template<> inline const uint8_t * TopicTraits<::msgs::LowCmd>::type_map_blob() {
|
||||
static const uint8_t blob[] = {
|
||||
0xc7, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0xf1, 0x99, 0xbb, 0x1c, 0xa1, 0x69, 0x8f, 0x02,
|
||||
0x5f, 0xc8, 0x64, 0x19, 0x21, 0x0b, 0x6a, 0x00, 0xaf, 0x00, 0x00, 0x00, 0xf1, 0x51, 0x01, 0x00,
|
||||
0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x9f, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00,
|
||||
0x16, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00,
|
||||
0x01, 0x00, 0x00, 0x00, 0x0c, 0x09, 0x76, 0x94, 0xf4, 0xa6, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00,
|
||||
0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00,
|
||||
0x0c, 0x09, 0x47, 0xbc, 0xdc, 0xd7, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00,
|
||||
0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x09, 0x16, 0xfc,
|
||||
0x75, 0xc7, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3,
|
||||
0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x09, 0x26, 0xb5, 0x68, 0xe4, 0x00, 0x00,
|
||||
0x16, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00,
|
||||
0x01, 0x00, 0x00, 0x00, 0x0c, 0x09, 0x82, 0xd0, 0x91, 0x47, 0x00, 0x00, 0x0b, 0x00, 0x00, 0x00,
|
||||
0x05, 0x00, 0x00, 0x00, 0x01, 0x00, 0x02, 0x41, 0x60, 0xe3, 0x55, 0x00, 0x0d, 0x01, 0x00, 0x00,
|
||||
0x01, 0x00, 0x00, 0x00, 0xf2, 0x2b, 0x33, 0x3f, 0xba, 0x68, 0xf3, 0xc8, 0xf8, 0xfe, 0xcd, 0x5a,
|
||||
0x4f, 0x81, 0xbf, 0x00, 0xf5, 0x00, 0x00, 0x00, 0xf2, 0x51, 0x01, 0x00, 0x15, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x0d, 0x00, 0x00, 0x00, 0x6d, 0x73, 0x67, 0x73, 0x3a, 0x3a, 0x4c, 0x6f,
|
||||
0x77, 0x43, 0x6d, 0x64, 0x00, 0x00, 0x00, 0x00, 0xd1, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00,
|
||||
0x1c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00,
|
||||
0x01, 0x00, 0x00, 0x00, 0x0c, 0x09, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x71, 0x00, 0x00, 0x00,
|
||||
0x1d, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00,
|
||||
0x01, 0x00, 0x00, 0x00, 0x0c, 0x09, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x64, 0x71, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x21, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3,
|
||||
0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x09, 0x00, 0x00, 0x07, 0x00, 0x00, 0x00,
|
||||
0x74, 0x61, 0x75, 0x5f, 0x66, 0x66, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0x00, 0x00, 0x00,
|
||||
0x03, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00,
|
||||
0x0c, 0x09, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x6b, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x1d, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00,
|
||||
0x01, 0x00, 0x00, 0x00, 0x0c, 0x09, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x6b, 0x76, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x15, 0x00, 0x00, 0x00, 0x05, 0x00, 0x00, 0x00, 0x01, 0x00, 0x02, 0x00,
|
||||
0x07, 0x00, 0x00, 0x00, 0x65, 0x5f, 0x73, 0x74, 0x6f, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x22, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0xf2, 0x2b, 0x33, 0x3f, 0xba, 0x68, 0xf3, 0xc8,
|
||||
0xf8, 0xfe, 0xcd, 0x5a, 0x4f, 0x81, 0xbf, 0xf1, 0x99, 0xbb, 0x1c, 0xa1, 0x69, 0x8f, 0x02, 0x5f,
|
||||
0xc8, 0x64, 0x19, 0x21, 0x0b, 0x6a, };
|
||||
return blob;
|
||||
}
|
||||
template<> inline const uint8_t * TopicTraits<::msgs::LowCmd>::type_info_blob() {
|
||||
static const uint8_t blob[] = {
|
||||
0x60, 0x00, 0x00, 0x00, 0x01, 0x10, 0x00, 0x40, 0x28, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00,
|
||||
0x14, 0x00, 0x00, 0x00, 0xf1, 0x99, 0xbb, 0x1c, 0xa1, 0x69, 0x8f, 0x02, 0x5f, 0xc8, 0x64, 0x19,
|
||||
0x21, 0x0b, 0x6a, 0x00, 0xb3, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x02, 0x10, 0x00, 0x40, 0x28, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00,
|
||||
0x14, 0x00, 0x00, 0x00, 0xf2, 0x2b, 0x33, 0x3f, 0xba, 0x68, 0xf3, 0xc8, 0xf8, 0xfe, 0xcd, 0x5a,
|
||||
0x4f, 0x81, 0xbf, 0x00, 0xf9, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, };
|
||||
return blob;
|
||||
}
|
||||
#endif //DDSCXX_HAS_TYPE_DISCOVERY
|
||||
|
||||
} //namespace topic
|
||||
} //namespace cyclonedds
|
||||
} //namespace eclipse
|
||||
} //namespace org
|
||||
|
||||
namespace dds {
|
||||
namespace topic {
|
||||
|
||||
template <>
|
||||
struct topic_type_name<::msgs::LowCmd>
|
||||
{
|
||||
static std::string value()
|
||||
{
|
||||
return org::eclipse::cyclonedds::topic::TopicTraits<::msgs::LowCmd>::getTypeName();
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
REGISTER_TOPIC_TYPE(::msgs::LowCmd)
|
||||
|
||||
namespace org{
|
||||
namespace eclipse{
|
||||
namespace cyclonedds{
|
||||
namespace core{
|
||||
namespace cdr{
|
||||
|
||||
template<>
|
||||
propvec &get_type_props<::msgs::LowCmd>();
|
||||
|
||||
template<typename T, std::enable_if_t<std::is_base_of<cdr_stream, T>::value, bool> = true >
|
||||
bool write(T& streamer, const ::msgs::LowCmd& instance, entity_properties_t *props) {
|
||||
(void)instance;
|
||||
if (!streamer.start_struct(*props))
|
||||
return false;
|
||||
auto prop = streamer.first_entity(props);
|
||||
while (prop) {
|
||||
switch (prop->m_id) {
|
||||
case 0:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!write(streamer, instance.q()[0], instance.q().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 1:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!write(streamer, instance.dq()[0], instance.dq().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 2:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!write(streamer, instance.tau_ff()[0], instance.tau_ff().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 3:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!write(streamer, instance.kp()[0], instance.kp().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 4:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!write(streamer, instance.kv()[0], instance.kv().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 5:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!write(streamer, instance.e_stop()))
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
prop = streamer.next_entity(prop);
|
||||
}
|
||||
return streamer.finish_struct(*props);
|
||||
}
|
||||
|
||||
template<typename S, std::enable_if_t<std::is_base_of<cdr_stream, S>::value, bool> = true >
|
||||
bool write(S& str, const ::msgs::LowCmd& instance, bool as_key) {
|
||||
auto &props = get_type_props<::msgs::LowCmd>();
|
||||
str.set_mode(cdr_stream::stream_mode::write, as_key);
|
||||
return write(str, instance, props.data());
|
||||
}
|
||||
|
||||
template<typename T, std::enable_if_t<std::is_base_of<cdr_stream, T>::value, bool> = true >
|
||||
bool read(T& streamer, ::msgs::LowCmd& instance, entity_properties_t *props) {
|
||||
(void)instance;
|
||||
if (!streamer.start_struct(*props))
|
||||
return false;
|
||||
auto prop = streamer.first_entity(props);
|
||||
while (prop) {
|
||||
switch (prop->m_id) {
|
||||
case 0:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!read(streamer, instance.q()[0], instance.q().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 1:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!read(streamer, instance.dq()[0], instance.dq().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 2:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!read(streamer, instance.tau_ff()[0], instance.tau_ff().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 3:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!read(streamer, instance.kp()[0], instance.kp().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 4:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!read(streamer, instance.kv()[0], instance.kv().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 5:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!read(streamer, instance.e_stop()))
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
prop = streamer.next_entity(prop);
|
||||
}
|
||||
return streamer.finish_struct(*props);
|
||||
}
|
||||
|
||||
template<typename S, std::enable_if_t<std::is_base_of<cdr_stream, S>::value, bool> = true >
|
||||
bool read(S& str, ::msgs::LowCmd& instance, bool as_key) {
|
||||
auto &props = get_type_props<::msgs::LowCmd>();
|
||||
str.set_mode(cdr_stream::stream_mode::read, as_key);
|
||||
return read(str, instance, props.data());
|
||||
}
|
||||
|
||||
template<typename T, std::enable_if_t<std::is_base_of<cdr_stream, T>::value, bool> = true >
|
||||
bool move(T& streamer, const ::msgs::LowCmd& instance, entity_properties_t *props) {
|
||||
(void)instance;
|
||||
if (!streamer.start_struct(*props))
|
||||
return false;
|
||||
auto prop = streamer.first_entity(props);
|
||||
while (prop) {
|
||||
switch (prop->m_id) {
|
||||
case 0:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!move(streamer, instance.q()[0], instance.q().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 1:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!move(streamer, instance.dq()[0], instance.dq().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 2:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!move(streamer, instance.tau_ff()[0], instance.tau_ff().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 3:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!move(streamer, instance.kp()[0], instance.kp().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 4:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!move(streamer, instance.kv()[0], instance.kv().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 5:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!move(streamer, instance.e_stop()))
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
prop = streamer.next_entity(prop);
|
||||
}
|
||||
return streamer.finish_struct(*props);
|
||||
}
|
||||
|
||||
template<typename S, std::enable_if_t<std::is_base_of<cdr_stream, S>::value, bool> = true >
|
||||
bool move(S& str, const ::msgs::LowCmd& instance, bool as_key) {
|
||||
auto &props = get_type_props<::msgs::LowCmd>();
|
||||
str.set_mode(cdr_stream::stream_mode::move, as_key);
|
||||
return move(str, instance, props.data());
|
||||
}
|
||||
|
||||
template<typename T, std::enable_if_t<std::is_base_of<cdr_stream, T>::value, bool> = true >
|
||||
bool max(T& streamer, const ::msgs::LowCmd& instance, entity_properties_t *props) {
|
||||
(void)instance;
|
||||
if (!streamer.start_struct(*props))
|
||||
return false;
|
||||
auto prop = streamer.first_entity(props);
|
||||
while (prop) {
|
||||
switch (prop->m_id) {
|
||||
case 0:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!max(streamer, instance.q()[0], instance.q().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 1:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!max(streamer, instance.dq()[0], instance.dq().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 2:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!max(streamer, instance.tau_ff()[0], instance.tau_ff().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 3:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!max(streamer, instance.kp()[0], instance.kp().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 4:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!max(streamer, instance.kv()[0], instance.kv().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 5:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!max(streamer, instance.e_stop()))
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
prop = streamer.next_entity(prop);
|
||||
}
|
||||
return streamer.finish_struct(*props);
|
||||
}
|
||||
|
||||
template<typename S, std::enable_if_t<std::is_base_of<cdr_stream, S>::value, bool> = true >
|
||||
bool max(S& str, const ::msgs::LowCmd& instance, bool as_key) {
|
||||
auto &props = get_type_props<::msgs::LowCmd>();
|
||||
str.set_mode(cdr_stream::stream_mode::max, as_key);
|
||||
return max(str, instance, props.data());
|
||||
}
|
||||
|
||||
} //namespace cdr
|
||||
} //namespace core
|
||||
} //namespace cyclonedds
|
||||
} //namespace eclipse
|
||||
} //namespace org
|
||||
|
||||
#endif // DDSCXX_LOWCMD_HPP
|
||||
|
|
@ -1,65 +0,0 @@
|
|||
/****************************************************************
|
||||
|
||||
Generated by Eclipse Cyclone DDS IDL to CXX Translator
|
||||
File name: LowState.idl
|
||||
Source: LowState.cpp
|
||||
Cyclone DDS: v0.10.2
|
||||
|
||||
*****************************************************************/
|
||||
#include "LowState.hpp"
|
||||
|
||||
namespace org{
|
||||
namespace eclipse{
|
||||
namespace cyclonedds{
|
||||
namespace core{
|
||||
namespace cdr{
|
||||
|
||||
template<>
|
||||
propvec &get_type_props<::msgs::LowState>() {
|
||||
static thread_local std::mutex mtx;
|
||||
static thread_local propvec props;
|
||||
static thread_local entity_properties_t *props_end = nullptr;
|
||||
static thread_local std::atomic_bool initialized {false};
|
||||
key_endpoint keylist;
|
||||
if (initialized.load(std::memory_order_relaxed)) {
|
||||
auto ptr = props.data();
|
||||
while (ptr < props_end)
|
||||
(ptr++)->is_present = false;
|
||||
return props;
|
||||
}
|
||||
std::lock_guard<std::mutex> lock(mtx);
|
||||
if (initialized.load(std::memory_order_relaxed)) {
|
||||
auto ptr = props.data();
|
||||
while (ptr < props_end)
|
||||
(ptr++)->is_present = false;
|
||||
return props;
|
||||
}
|
||||
props.clear();
|
||||
|
||||
props.push_back(entity_properties_t(0, 0, false, bb_unset, extensibility::ext_final)); //root
|
||||
props.push_back(entity_properties_t(1, 0, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::q
|
||||
props.push_back(entity_properties_t(1, 1, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::dq
|
||||
props.push_back(entity_properties_t(1, 2, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::ddq
|
||||
props.push_back(entity_properties_t(1, 3, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::tau_est
|
||||
props.push_back(entity_properties_t(1, 4, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::tmp
|
||||
props.push_back(entity_properties_t(1, 5, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::contact
|
||||
props.push_back(entity_properties_t(1, 6, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::quat
|
||||
props.push_back(entity_properties_t(1, 7, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::gyro
|
||||
props.push_back(entity_properties_t(1, 8, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::accel
|
||||
props.push_back(entity_properties_t(1, 9, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::rpy
|
||||
props.push_back(entity_properties_t(1, 10, false, get_bit_bound<uint8_t>(), extensibility::ext_final, false)); //::imu_tmp
|
||||
props.push_back(entity_properties_t(1, 11, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::voltage
|
||||
props.push_back(entity_properties_t(1, 12, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::current
|
||||
|
||||
entity_properties_t::finish(props, keylist);
|
||||
props_end = props.data() + props.size();
|
||||
initialized.store(true, std::memory_order_release);
|
||||
return props;
|
||||
}
|
||||
|
||||
} //namespace cdr
|
||||
} //namespace core
|
||||
} //namespace cyclonedds
|
||||
} //namespace eclipse
|
||||
} //namespace org
|
||||
|
||||
|
|
@ -1,936 +0,0 @@
|
|||
/****************************************************************
|
||||
|
||||
Generated by Eclipse Cyclone DDS IDL to CXX Translator
|
||||
File name: LowState.idl
|
||||
Source: LowState.hpp
|
||||
Cyclone DDS: v0.10.2
|
||||
|
||||
*****************************************************************/
|
||||
#ifndef DDSCXX_LOWSTATE_HPP
|
||||
#define DDSCXX_LOWSTATE_HPP
|
||||
|
||||
#include <cstdint>
|
||||
#include <array>
|
||||
|
||||
namespace msgs
|
||||
{
|
||||
class LowState
|
||||
{
|
||||
private:
|
||||
std::array<float, 12> q_ = { };
|
||||
std::array<float, 12> dq_ = { };
|
||||
std::array<float, 12> ddq_ = { };
|
||||
std::array<float, 12> tau_est_ = { };
|
||||
std::array<float, 12> tmp_ = { };
|
||||
std::array<float, 4> contact_ = { };
|
||||
std::array<float, 4> quat_ = { };
|
||||
std::array<float, 3> gyro_ = { };
|
||||
std::array<float, 3> accel_ = { };
|
||||
std::array<float, 3> rpy_ = { };
|
||||
uint8_t imu_tmp_ = 0;
|
||||
float voltage_ = 0.0f;
|
||||
float current_ = 0.0f;
|
||||
|
||||
public:
|
||||
LowState() = default;
|
||||
|
||||
explicit LowState(
|
||||
const std::array<float, 12>& q,
|
||||
const std::array<float, 12>& dq,
|
||||
const std::array<float, 12>& ddq,
|
||||
const std::array<float, 12>& tau_est,
|
||||
const std::array<float, 12>& tmp,
|
||||
const std::array<float, 4>& contact,
|
||||
const std::array<float, 4>& quat,
|
||||
const std::array<float, 3>& gyro,
|
||||
const std::array<float, 3>& accel,
|
||||
const std::array<float, 3>& rpy,
|
||||
uint8_t imu_tmp,
|
||||
float voltage,
|
||||
float current) :
|
||||
q_(q),
|
||||
dq_(dq),
|
||||
ddq_(ddq),
|
||||
tau_est_(tau_est),
|
||||
tmp_(tmp),
|
||||
contact_(contact),
|
||||
quat_(quat),
|
||||
gyro_(gyro),
|
||||
accel_(accel),
|
||||
rpy_(rpy),
|
||||
imu_tmp_(imu_tmp),
|
||||
voltage_(voltage),
|
||||
current_(current) { }
|
||||
|
||||
const std::array<float, 12>& q() const { return this->q_; }
|
||||
std::array<float, 12>& q() { return this->q_; }
|
||||
void q(const std::array<float, 12>& _val_) { this->q_ = _val_; }
|
||||
void q(std::array<float, 12>&& _val_) { this->q_ = _val_; }
|
||||
const std::array<float, 12>& dq() const { return this->dq_; }
|
||||
std::array<float, 12>& dq() { return this->dq_; }
|
||||
void dq(const std::array<float, 12>& _val_) { this->dq_ = _val_; }
|
||||
void dq(std::array<float, 12>&& _val_) { this->dq_ = _val_; }
|
||||
const std::array<float, 12>& ddq() const { return this->ddq_; }
|
||||
std::array<float, 12>& ddq() { return this->ddq_; }
|
||||
void ddq(const std::array<float, 12>& _val_) { this->ddq_ = _val_; }
|
||||
void ddq(std::array<float, 12>&& _val_) { this->ddq_ = _val_; }
|
||||
const std::array<float, 12>& tau_est() const { return this->tau_est_; }
|
||||
std::array<float, 12>& tau_est() { return this->tau_est_; }
|
||||
void tau_est(const std::array<float, 12>& _val_) { this->tau_est_ = _val_; }
|
||||
void tau_est(std::array<float, 12>&& _val_) { this->tau_est_ = _val_; }
|
||||
const std::array<float, 12>& tmp() const { return this->tmp_; }
|
||||
std::array<float, 12>& tmp() { return this->tmp_; }
|
||||
void tmp(const std::array<float, 12>& _val_) { this->tmp_ = _val_; }
|
||||
void tmp(std::array<float, 12>&& _val_) { this->tmp_ = _val_; }
|
||||
const std::array<float, 4>& contact() const { return this->contact_; }
|
||||
std::array<float, 4>& contact() { return this->contact_; }
|
||||
void contact(const std::array<float, 4>& _val_) { this->contact_ = _val_; }
|
||||
void contact(std::array<float, 4>&& _val_) { this->contact_ = _val_; }
|
||||
const std::array<float, 4>& quat() const { return this->quat_; }
|
||||
std::array<float, 4>& quat() { return this->quat_; }
|
||||
void quat(const std::array<float, 4>& _val_) { this->quat_ = _val_; }
|
||||
void quat(std::array<float, 4>&& _val_) { this->quat_ = _val_; }
|
||||
const std::array<float, 3>& gyro() const { return this->gyro_; }
|
||||
std::array<float, 3>& gyro() { return this->gyro_; }
|
||||
void gyro(const std::array<float, 3>& _val_) { this->gyro_ = _val_; }
|
||||
void gyro(std::array<float, 3>&& _val_) { this->gyro_ = _val_; }
|
||||
const std::array<float, 3>& accel() const { return this->accel_; }
|
||||
std::array<float, 3>& accel() { return this->accel_; }
|
||||
void accel(const std::array<float, 3>& _val_) { this->accel_ = _val_; }
|
||||
void accel(std::array<float, 3>&& _val_) { this->accel_ = _val_; }
|
||||
const std::array<float, 3>& rpy() const { return this->rpy_; }
|
||||
std::array<float, 3>& rpy() { return this->rpy_; }
|
||||
void rpy(const std::array<float, 3>& _val_) { this->rpy_ = _val_; }
|
||||
void rpy(std::array<float, 3>&& _val_) { this->rpy_ = _val_; }
|
||||
uint8_t imu_tmp() const { return this->imu_tmp_; }
|
||||
uint8_t& imu_tmp() { return this->imu_tmp_; }
|
||||
void imu_tmp(uint8_t _val_) { this->imu_tmp_ = _val_; }
|
||||
float voltage() const { return this->voltage_; }
|
||||
float& voltage() { return this->voltage_; }
|
||||
void voltage(float _val_) { this->voltage_ = _val_; }
|
||||
float current() const { return this->current_; }
|
||||
float& current() { return this->current_; }
|
||||
void current(float _val_) { this->current_ = _val_; }
|
||||
|
||||
bool operator==(const LowState& _other) const
|
||||
{
|
||||
(void) _other;
|
||||
return q_ == _other.q_ &&
|
||||
dq_ == _other.dq_ &&
|
||||
ddq_ == _other.ddq_ &&
|
||||
tau_est_ == _other.tau_est_ &&
|
||||
tmp_ == _other.tmp_ &&
|
||||
contact_ == _other.contact_ &&
|
||||
quat_ == _other.quat_ &&
|
||||
gyro_ == _other.gyro_ &&
|
||||
accel_ == _other.accel_ &&
|
||||
rpy_ == _other.rpy_ &&
|
||||
imu_tmp_ == _other.imu_tmp_ &&
|
||||
voltage_ == _other.voltage_ &&
|
||||
current_ == _other.current_;
|
||||
}
|
||||
|
||||
bool operator!=(const LowState& _other) const
|
||||
{
|
||||
return !(*this == _other);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#include "dds/topic/TopicTraits.hpp"
|
||||
#include "org/eclipse/cyclonedds/topic/datatopic.hpp"
|
||||
|
||||
namespace org {
|
||||
namespace eclipse {
|
||||
namespace cyclonedds {
|
||||
namespace topic {
|
||||
|
||||
template <> constexpr const char* TopicTraits<::msgs::LowState>::getTypeName()
|
||||
{
|
||||
return "msgs::LowState";
|
||||
}
|
||||
|
||||
template <> constexpr bool TopicTraits<::msgs::LowState>::isKeyless()
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
#ifdef DDSCXX_HAS_TYPE_DISCOVERY
|
||||
template<> constexpr unsigned int TopicTraits<::msgs::LowState>::type_map_blob_sz() { return 930; }
|
||||
template<> constexpr unsigned int TopicTraits<::msgs::LowState>::type_info_blob_sz() { return 100; }
|
||||
template<> inline const uint8_t * TopicTraits<::msgs::LowState>::type_map_blob() {
|
||||
static const uint8_t blob[] = {
|
||||
0x73, 0x01, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0xf1, 0xec, 0xa8, 0x61, 0xd2, 0xd7, 0x0d, 0xa1,
|
||||
0x5b, 0xdc, 0x10, 0x55, 0xb8, 0xdc, 0x20, 0x00, 0x5b, 0x01, 0x00, 0x00, 0xf1, 0x51, 0x01, 0x00,
|
||||
0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x4b, 0x01, 0x00, 0x00, 0x0d, 0x00, 0x00, 0x00,
|
||||
0x16, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00,
|
||||
0x01, 0x00, 0x00, 0x00, 0x0c, 0x09, 0x76, 0x94, 0xf4, 0xa6, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00,
|
||||
0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00,
|
||||
0x0c, 0x09, 0x47, 0xbc, 0xdc, 0xd7, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00,
|
||||
0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x09, 0xe9, 0x16,
|
||||
0x89, 0x09, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3,
|
||||
0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x09, 0x8a, 0xf7, 0xae, 0xdf, 0x00, 0x00,
|
||||
0x16, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00,
|
||||
0x01, 0x00, 0x00, 0x00, 0x0c, 0x09, 0xfa, 0x81, 0x6e, 0xdb, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00,
|
||||
0x05, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00,
|
||||
0x04, 0x09, 0x2f, 0x8a, 0x6b, 0xf3, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00,
|
||||
0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x09, 0x21, 0xd7,
|
||||
0xdc, 0x6a, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3,
|
||||
0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x03, 0x09, 0x41, 0xe1, 0xdb, 0x58, 0x00, 0x00,
|
||||
0x16, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00,
|
||||
0x01, 0x00, 0x00, 0x00, 0x03, 0x09, 0x98, 0x30, 0x99, 0x65, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00,
|
||||
0x09, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00,
|
||||
0x03, 0x09, 0xb0, 0x7d, 0x92, 0xed, 0x00, 0x00, 0x0b, 0x00, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00,
|
||||
0x01, 0x00, 0x02, 0x04, 0x25, 0x70, 0x6e, 0x00, 0x0b, 0x00, 0x00, 0x00, 0x0b, 0x00, 0x00, 0x00,
|
||||
0x01, 0x00, 0x09, 0xe4, 0x37, 0xba, 0x43, 0x00, 0x0b, 0x00, 0x00, 0x00, 0x0c, 0x00, 0x00, 0x00,
|
||||
0x01, 0x00, 0x09, 0x43, 0xb5, 0xc9, 0x17, 0x00, 0xfe, 0x01, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00,
|
||||
0xf2, 0xf7, 0x0b, 0xf4, 0x7b, 0xfb, 0x17, 0xea, 0xce, 0xcb, 0x7f, 0x4c, 0xbc, 0x81, 0x4c, 0x00,
|
||||
0xe6, 0x01, 0x00, 0x00, 0xf2, 0x51, 0x01, 0x00, 0x17, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x0f, 0x00, 0x00, 0x00, 0x6d, 0x73, 0x67, 0x73, 0x3a, 0x3a, 0x4c, 0x6f, 0x77, 0x53, 0x74, 0x61,
|
||||
0x74, 0x65, 0x00, 0x00, 0xc2, 0x01, 0x00, 0x00, 0x0d, 0x00, 0x00, 0x00, 0x1c, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00,
|
||||
0x0c, 0x09, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x71, 0x00, 0x00, 0x00, 0x1d, 0x00, 0x00, 0x00,
|
||||
0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00,
|
||||
0x0c, 0x09, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x64, 0x71, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x1e, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00,
|
||||
0x01, 0x00, 0x00, 0x00, 0x0c, 0x09, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x64, 0x64, 0x71, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x22, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3,
|
||||
0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x09, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00,
|
||||
0x74, 0x61, 0x75, 0x5f, 0x65, 0x73, 0x74, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1e, 0x00, 0x00, 0x00,
|
||||
0x04, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00,
|
||||
0x0c, 0x09, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x74, 0x6d, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x22, 0x00, 0x00, 0x00, 0x05, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00,
|
||||
0x01, 0x00, 0x00, 0x00, 0x04, 0x09, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x63, 0x6f, 0x6e, 0x74,
|
||||
0x61, 0x63, 0x74, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1f, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00,
|
||||
0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x09, 0x00, 0x00,
|
||||
0x05, 0x00, 0x00, 0x00, 0x71, 0x75, 0x61, 0x74, 0x00, 0x00, 0x00, 0x00, 0x1f, 0x00, 0x00, 0x00,
|
||||
0x07, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00,
|
||||
0x03, 0x09, 0x00, 0x00, 0x05, 0x00, 0x00, 0x00, 0x67, 0x79, 0x72, 0x6f, 0x00, 0x00, 0x00, 0x00,
|
||||
0x20, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00,
|
||||
0x01, 0x00, 0x00, 0x00, 0x03, 0x09, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x61, 0x63, 0x63, 0x65,
|
||||
0x6c, 0x00, 0x00, 0x00, 0x1e, 0x00, 0x00, 0x00, 0x09, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3,
|
||||
0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x03, 0x09, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00,
|
||||
0x72, 0x70, 0x79, 0x00, 0x00, 0x00, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00,
|
||||
0x01, 0x00, 0x02, 0x00, 0x08, 0x00, 0x00, 0x00, 0x69, 0x6d, 0x75, 0x5f, 0x74, 0x6d, 0x70, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00, 0x0b, 0x00, 0x00, 0x00, 0x01, 0x00, 0x09, 0x00,
|
||||
0x08, 0x00, 0x00, 0x00, 0x76, 0x6f, 0x6c, 0x74, 0x61, 0x67, 0x65, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x16, 0x00, 0x00, 0x00, 0x0c, 0x00, 0x00, 0x00, 0x01, 0x00, 0x09, 0x00, 0x08, 0x00, 0x00, 0x00,
|
||||
0x63, 0x75, 0x72, 0x72, 0x65, 0x6e, 0x74, 0x00, 0x00, 0x00, 0x00, 0x00, 0x22, 0x00, 0x00, 0x00,
|
||||
0x01, 0x00, 0x00, 0x00, 0xf2, 0xf7, 0x0b, 0xf4, 0x7b, 0xfb, 0x17, 0xea, 0xce, 0xcb, 0x7f, 0x4c,
|
||||
0xbc, 0x81, 0x4c, 0xf1, 0xec, 0xa8, 0x61, 0xd2, 0xd7, 0x0d, 0xa1, 0x5b, 0xdc, 0x10, 0x55, 0xb8,
|
||||
0xdc, 0x20, };
|
||||
return blob;
|
||||
}
|
||||
template<> inline const uint8_t * TopicTraits<::msgs::LowState>::type_info_blob() {
|
||||
static const uint8_t blob[] = {
|
||||
0x60, 0x00, 0x00, 0x00, 0x01, 0x10, 0x00, 0x40, 0x28, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00,
|
||||
0x14, 0x00, 0x00, 0x00, 0xf1, 0xec, 0xa8, 0x61, 0xd2, 0xd7, 0x0d, 0xa1, 0x5b, 0xdc, 0x10, 0x55,
|
||||
0xb8, 0xdc, 0x20, 0x00, 0x5f, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x02, 0x10, 0x00, 0x40, 0x28, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00,
|
||||
0x14, 0x00, 0x00, 0x00, 0xf2, 0xf7, 0x0b, 0xf4, 0x7b, 0xfb, 0x17, 0xea, 0xce, 0xcb, 0x7f, 0x4c,
|
||||
0xbc, 0x81, 0x4c, 0x00, 0xea, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, };
|
||||
return blob;
|
||||
}
|
||||
#endif //DDSCXX_HAS_TYPE_DISCOVERY
|
||||
|
||||
} //namespace topic
|
||||
} //namespace cyclonedds
|
||||
} //namespace eclipse
|
||||
} //namespace org
|
||||
|
||||
namespace dds {
|
||||
namespace topic {
|
||||
|
||||
template <>
|
||||
struct topic_type_name<::msgs::LowState>
|
||||
{
|
||||
static std::string value()
|
||||
{
|
||||
return org::eclipse::cyclonedds::topic::TopicTraits<::msgs::LowState>::getTypeName();
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
REGISTER_TOPIC_TYPE(::msgs::LowState)
|
||||
|
||||
namespace org{
|
||||
namespace eclipse{
|
||||
namespace cyclonedds{
|
||||
namespace core{
|
||||
namespace cdr{
|
||||
|
||||
template<>
|
||||
propvec &get_type_props<::msgs::LowState>();
|
||||
|
||||
template<typename T, std::enable_if_t<std::is_base_of<cdr_stream, T>::value, bool> = true >
|
||||
bool write(T& streamer, const ::msgs::LowState& instance, entity_properties_t *props) {
|
||||
(void)instance;
|
||||
if (!streamer.start_struct(*props))
|
||||
return false;
|
||||
auto prop = streamer.first_entity(props);
|
||||
while (prop) {
|
||||
switch (prop->m_id) {
|
||||
case 0:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!write(streamer, instance.q()[0], instance.q().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 1:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!write(streamer, instance.dq()[0], instance.dq().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 2:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!write(streamer, instance.ddq()[0], instance.ddq().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 3:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!write(streamer, instance.tau_est()[0], instance.tau_est().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 4:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!write(streamer, instance.tmp()[0], instance.tmp().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 5:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!write(streamer, instance.contact()[0], instance.contact().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 6:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!write(streamer, instance.quat()[0], instance.quat().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 7:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!write(streamer, instance.gyro()[0], instance.gyro().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 8:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!write(streamer, instance.accel()[0], instance.accel().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 9:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!write(streamer, instance.rpy()[0], instance.rpy().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 10:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!write(streamer, instance.imu_tmp()))
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 11:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!write(streamer, instance.voltage()))
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 12:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!write(streamer, instance.current()))
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
prop = streamer.next_entity(prop);
|
||||
}
|
||||
return streamer.finish_struct(*props);
|
||||
}
|
||||
|
||||
template<typename S, std::enable_if_t<std::is_base_of<cdr_stream, S>::value, bool> = true >
|
||||
bool write(S& str, const ::msgs::LowState& instance, bool as_key) {
|
||||
auto &props = get_type_props<::msgs::LowState>();
|
||||
str.set_mode(cdr_stream::stream_mode::write, as_key);
|
||||
return write(str, instance, props.data());
|
||||
}
|
||||
|
||||
template<typename T, std::enable_if_t<std::is_base_of<cdr_stream, T>::value, bool> = true >
|
||||
bool read(T& streamer, ::msgs::LowState& instance, entity_properties_t *props) {
|
||||
(void)instance;
|
||||
if (!streamer.start_struct(*props))
|
||||
return false;
|
||||
auto prop = streamer.first_entity(props);
|
||||
while (prop) {
|
||||
switch (prop->m_id) {
|
||||
case 0:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!read(streamer, instance.q()[0], instance.q().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 1:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!read(streamer, instance.dq()[0], instance.dq().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 2:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!read(streamer, instance.ddq()[0], instance.ddq().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 3:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!read(streamer, instance.tau_est()[0], instance.tau_est().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 4:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!read(streamer, instance.tmp()[0], instance.tmp().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 5:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!read(streamer, instance.contact()[0], instance.contact().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 6:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!read(streamer, instance.quat()[0], instance.quat().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 7:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!read(streamer, instance.gyro()[0], instance.gyro().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 8:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!read(streamer, instance.accel()[0], instance.accel().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 9:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!read(streamer, instance.rpy()[0], instance.rpy().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 10:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!read(streamer, instance.imu_tmp()))
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 11:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!read(streamer, instance.voltage()))
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 12:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!read(streamer, instance.current()))
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
prop = streamer.next_entity(prop);
|
||||
}
|
||||
return streamer.finish_struct(*props);
|
||||
}
|
||||
|
||||
template<typename S, std::enable_if_t<std::is_base_of<cdr_stream, S>::value, bool> = true >
|
||||
bool read(S& str, ::msgs::LowState& instance, bool as_key) {
|
||||
auto &props = get_type_props<::msgs::LowState>();
|
||||
str.set_mode(cdr_stream::stream_mode::read, as_key);
|
||||
return read(str, instance, props.data());
|
||||
}
|
||||
|
||||
template<typename T, std::enable_if_t<std::is_base_of<cdr_stream, T>::value, bool> = true >
|
||||
bool move(T& streamer, const ::msgs::LowState& instance, entity_properties_t *props) {
|
||||
(void)instance;
|
||||
if (!streamer.start_struct(*props))
|
||||
return false;
|
||||
auto prop = streamer.first_entity(props);
|
||||
while (prop) {
|
||||
switch (prop->m_id) {
|
||||
case 0:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!move(streamer, instance.q()[0], instance.q().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 1:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!move(streamer, instance.dq()[0], instance.dq().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 2:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!move(streamer, instance.ddq()[0], instance.ddq().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 3:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!move(streamer, instance.tau_est()[0], instance.tau_est().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 4:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!move(streamer, instance.tmp()[0], instance.tmp().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 5:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!move(streamer, instance.contact()[0], instance.contact().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 6:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!move(streamer, instance.quat()[0], instance.quat().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 7:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!move(streamer, instance.gyro()[0], instance.gyro().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 8:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!move(streamer, instance.accel()[0], instance.accel().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 9:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!move(streamer, instance.rpy()[0], instance.rpy().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 10:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!move(streamer, instance.imu_tmp()))
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 11:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!move(streamer, instance.voltage()))
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 12:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!move(streamer, instance.current()))
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
prop = streamer.next_entity(prop);
|
||||
}
|
||||
return streamer.finish_struct(*props);
|
||||
}
|
||||
|
||||
template<typename S, std::enable_if_t<std::is_base_of<cdr_stream, S>::value, bool> = true >
|
||||
bool move(S& str, const ::msgs::LowState& instance, bool as_key) {
|
||||
auto &props = get_type_props<::msgs::LowState>();
|
||||
str.set_mode(cdr_stream::stream_mode::move, as_key);
|
||||
return move(str, instance, props.data());
|
||||
}
|
||||
|
||||
template<typename T, std::enable_if_t<std::is_base_of<cdr_stream, T>::value, bool> = true >
|
||||
bool max(T& streamer, const ::msgs::LowState& instance, entity_properties_t *props) {
|
||||
(void)instance;
|
||||
if (!streamer.start_struct(*props))
|
||||
return false;
|
||||
auto prop = streamer.first_entity(props);
|
||||
while (prop) {
|
||||
switch (prop->m_id) {
|
||||
case 0:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!max(streamer, instance.q()[0], instance.q().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 1:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!max(streamer, instance.dq()[0], instance.dq().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 2:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!max(streamer, instance.ddq()[0], instance.ddq().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 3:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!max(streamer, instance.tau_est()[0], instance.tau_est().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 4:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!max(streamer, instance.tmp()[0], instance.tmp().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 5:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!max(streamer, instance.contact()[0], instance.contact().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 6:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!max(streamer, instance.quat()[0], instance.quat().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 7:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!max(streamer, instance.gyro()[0], instance.gyro().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 8:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!max(streamer, instance.accel()[0], instance.accel().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 9:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!streamer.start_consecutive(true, true))
|
||||
return false;
|
||||
if (!max(streamer, instance.rpy()[0], instance.rpy().size()))
|
||||
return false;
|
||||
if (!streamer.finish_consecutive())
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 10:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!max(streamer, instance.imu_tmp()))
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 11:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!max(streamer, instance.voltage()))
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
case 12:
|
||||
if (!streamer.start_member(*prop))
|
||||
return false;
|
||||
if (!max(streamer, instance.current()))
|
||||
return false;
|
||||
if (!streamer.finish_member(*prop))
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
prop = streamer.next_entity(prop);
|
||||
}
|
||||
return streamer.finish_struct(*props);
|
||||
}
|
||||
|
||||
template<typename S, std::enable_if_t<std::is_base_of<cdr_stream, S>::value, bool> = true >
|
||||
bool max(S& str, const ::msgs::LowState& instance, bool as_key) {
|
||||
auto &props = get_type_props<::msgs::LowState>();
|
||||
str.set_mode(cdr_stream::stream_mode::max, as_key);
|
||||
return max(str, instance, props.data());
|
||||
}
|
||||
|
||||
} //namespace cdr
|
||||
} //namespace core
|
||||
} //namespace cyclonedds
|
||||
} //namespace eclipse
|
||||
} //namespace org
|
||||
|
||||
#endif // DDSCXX_LOWSTATE_HPP
|
||||
|
|
@ -1,390 +0,0 @@
|
|||
#ifndef __UT_ANY_HPP__
|
||||
#define __UT_ANY_HPP__
|
||||
|
||||
#include <unitree/common/exception.hpp>
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
class Any
|
||||
{
|
||||
public:
|
||||
Any()
|
||||
: mContent(0)
|
||||
{}
|
||||
|
||||
template<typename ValueType>
|
||||
Any(const ValueType& value)
|
||||
: mContent(new Holder<ValueType>(value))
|
||||
{}
|
||||
|
||||
Any(const char* s)
|
||||
: Any(std::string(s))
|
||||
{}
|
||||
|
||||
Any(const char* s, size_t len)
|
||||
: Any(std::string(s, len))
|
||||
{}
|
||||
|
||||
Any(const Any& other)
|
||||
: mContent(other.mContent ? other.mContent->Clone() : 0)
|
||||
{}
|
||||
|
||||
~Any()
|
||||
{
|
||||
delete mContent;
|
||||
mContent = 0;
|
||||
}
|
||||
|
||||
Any& Swap(Any& other)
|
||||
{
|
||||
std::swap(mContent, other.mContent);
|
||||
return *this;
|
||||
}
|
||||
|
||||
bool Empty() const
|
||||
{
|
||||
return mContent == 0;
|
||||
}
|
||||
|
||||
const std::type_info& GetTypeInfo() const
|
||||
{
|
||||
return mContent ? mContent->GetTypeInfo() : typeid(void);
|
||||
}
|
||||
|
||||
template<typename ValueType>
|
||||
Any& operator=(const ValueType& other)
|
||||
{
|
||||
Any(other).Swap(*this);
|
||||
return *this;
|
||||
}
|
||||
|
||||
Any& operator=(Any other)
|
||||
{
|
||||
other.Swap(*this);
|
||||
return *this;
|
||||
}
|
||||
|
||||
public:
|
||||
class PlaceHolder
|
||||
{
|
||||
public:
|
||||
virtual ~PlaceHolder()
|
||||
{}
|
||||
|
||||
public:
|
||||
virtual const std::type_info& GetTypeInfo() const = 0;
|
||||
virtual PlaceHolder* Clone() const = 0;
|
||||
};
|
||||
|
||||
template<typename ValueType>
|
||||
class Holder : public PlaceHolder
|
||||
{
|
||||
public:
|
||||
explicit Holder(const ValueType& value)
|
||||
: mValue(value)
|
||||
{}
|
||||
|
||||
virtual const std::type_info& GetTypeInfo() const
|
||||
{
|
||||
return typeid(ValueType);
|
||||
}
|
||||
|
||||
virtual PlaceHolder* Clone() const
|
||||
{
|
||||
return new Holder(mValue);
|
||||
}
|
||||
|
||||
public:
|
||||
ValueType mValue;
|
||||
};
|
||||
|
||||
public:
|
||||
PlaceHolder* mContent;
|
||||
};
|
||||
|
||||
/*
|
||||
* static const Any
|
||||
*/
|
||||
static const Any UT_EMPTY_ANY = Any();
|
||||
|
||||
static inline bool IsBool(const Any& any)
|
||||
{
|
||||
return any.GetTypeInfo() == typeid(bool);
|
||||
}
|
||||
|
||||
static inline bool IsString(const Any& any)
|
||||
{
|
||||
return any.GetTypeInfo() == typeid(std::string);
|
||||
}
|
||||
|
||||
static inline bool IsInt8(const Any& any)
|
||||
{
|
||||
return any.GetTypeInfo() == typeid(int8_t);
|
||||
}
|
||||
|
||||
static inline bool IsUint8(const Any& any)
|
||||
{
|
||||
return any.GetTypeInfo() == typeid(uint8_t);
|
||||
}
|
||||
|
||||
static inline bool IsInt16(const Any& any)
|
||||
{
|
||||
return any.GetTypeInfo() == typeid(int16_t);
|
||||
}
|
||||
|
||||
static inline bool IsUint16(const Any& any)
|
||||
{
|
||||
return any.GetTypeInfo() == typeid(uint16_t);
|
||||
}
|
||||
|
||||
static inline bool IsInt(const Any& any)
|
||||
{
|
||||
return any.GetTypeInfo() == typeid(int32_t);
|
||||
}
|
||||
|
||||
static inline bool IsUint(const Any& any)
|
||||
{
|
||||
return any.GetTypeInfo() == typeid(uint32_t);
|
||||
}
|
||||
|
||||
static inline bool IsInt64(const Any& any)
|
||||
{
|
||||
return any.GetTypeInfo() == typeid(int64_t);
|
||||
}
|
||||
|
||||
static inline bool IsUint64(const Any& any)
|
||||
{
|
||||
return any.GetTypeInfo() == typeid(uint64_t);
|
||||
}
|
||||
|
||||
static inline bool IsFloat(const Any& any)
|
||||
{
|
||||
return any.GetTypeInfo() == typeid(float);
|
||||
}
|
||||
|
||||
static inline bool IsDouble(const Any& any)
|
||||
{
|
||||
return any.GetTypeInfo() == typeid(double);
|
||||
}
|
||||
|
||||
static inline bool IsLongDouble(const Any& any)
|
||||
{
|
||||
return any.GetTypeInfo() == typeid(long double);
|
||||
}
|
||||
|
||||
static inline bool IsInteger(const Any& any)
|
||||
{
|
||||
return IsInt(any) || IsUint(any) || IsInt64(any) || IsUint64(any)
|
||||
|| IsInt16(any) || IsUint16(any) || IsInt8(any) || IsUint8(any);
|
||||
}
|
||||
|
||||
static inline bool IsNumber(const Any& any)
|
||||
{
|
||||
return IsBool(any) || IsInteger(any) || IsFloat(any) || IsDouble(any)
|
||||
|| IsLongDouble(any);
|
||||
}
|
||||
|
||||
static inline bool IsBoolType(const std::type_info& t)
|
||||
{
|
||||
return t == typeid(bool);
|
||||
}
|
||||
|
||||
static inline bool IsInt8Type(const std::type_info& t)
|
||||
{
|
||||
return t == typeid(int8_t);
|
||||
}
|
||||
|
||||
static inline bool IsUint8Type(const std::type_info& t)
|
||||
{
|
||||
return t == typeid(uint8_t);
|
||||
}
|
||||
|
||||
static inline bool IsInt16Type(const std::type_info& t)
|
||||
{
|
||||
return t == typeid(int16_t);
|
||||
}
|
||||
|
||||
static inline bool IsUint16Type(const std::type_info& t)
|
||||
{
|
||||
return t == typeid(uint16_t);
|
||||
}
|
||||
|
||||
static inline bool IsIntType(const std::type_info& t)
|
||||
{
|
||||
return t == typeid(int32_t);
|
||||
}
|
||||
|
||||
static inline bool IsUintType(const std::type_info& t)
|
||||
{
|
||||
return t == typeid(uint32_t);
|
||||
}
|
||||
|
||||
static inline bool IsInt64Type(const std::type_info& t)
|
||||
{
|
||||
return t == typeid(int64_t);
|
||||
}
|
||||
|
||||
static inline bool IsUint64Type(const std::type_info& t)
|
||||
{
|
||||
return t == typeid(uint64_t);
|
||||
}
|
||||
|
||||
static inline bool IsIntegerType(const std::type_info& t)
|
||||
{
|
||||
return IsIntType(t) || IsUintType(t) || IsInt64Type(t) || IsUint64Type(t) ||
|
||||
IsInt8Type(t) || IsUint8Type(t) || IsInt16Type(t) || IsUint16Type(t);
|
||||
}
|
||||
|
||||
static inline bool IsFloatType(const std::type_info& t)
|
||||
{
|
||||
return t == typeid(float);
|
||||
}
|
||||
|
||||
static inline bool IsDoubleType(const std::type_info& t)
|
||||
{
|
||||
return t == typeid(double);
|
||||
}
|
||||
|
||||
static inline bool IsLongDoubleType(const std::type_info& t)
|
||||
{
|
||||
return t == typeid(long double);
|
||||
}
|
||||
|
||||
static inline bool IsNumberType(const std::type_info& t)
|
||||
{
|
||||
return IsBoolType(t) || IsIntegerType(t) || IsFloatType(t)
|
||||
|| IsDoubleType(t) || IsLongDoubleType(t);
|
||||
}
|
||||
|
||||
static inline bool IsTypeEqual(const std::type_info& t1, const std::type_info& t2)
|
||||
{
|
||||
return t1 == t2;
|
||||
}
|
||||
|
||||
template<typename ValueType>
|
||||
const ValueType& AnyCast(const Any* operand)
|
||||
{
|
||||
const std::type_info& t1 = typeid(ValueType);
|
||||
const std::type_info& t2 = operand->GetTypeInfo();
|
||||
|
||||
if (IsTypeEqual(t1, t2))
|
||||
{
|
||||
return ((Any::Holder<ValueType>*)(operand->mContent))->mValue;
|
||||
}
|
||||
|
||||
UT_THROW(BadCastException, std::string("AnyCast error. target type is ")
|
||||
+ t1.name() + ", but source type is " + t2.name());
|
||||
}
|
||||
|
||||
template<typename ValueType>
|
||||
const ValueType& AnyCast(const Any& operand)
|
||||
{
|
||||
return AnyCast<ValueType>(&operand);
|
||||
}
|
||||
|
||||
template<typename ValueType>
|
||||
ValueType AnyNumberCast(const Any* operand)
|
||||
{
|
||||
const std::type_info& t1 = typeid(ValueType);
|
||||
const std::type_info& t2 = operand->GetTypeInfo();
|
||||
|
||||
if (IsNumberType(t1) && IsNumberType(t2))
|
||||
{
|
||||
if (IsTypeEqual(t1, t2))
|
||||
{
|
||||
return ((Any::Holder<ValueType>*)(operand->mContent))->mValue;
|
||||
}
|
||||
else if (IsFloatType(t2))
|
||||
{
|
||||
return (ValueType)((Any::Holder<float>*)(operand->mContent))->mValue;
|
||||
}
|
||||
else if (IsDoubleType(t2))
|
||||
{
|
||||
return (ValueType)((Any::Holder<double>*)(operand->mContent))->mValue;
|
||||
}
|
||||
else if (IsLongDoubleType(t2))
|
||||
{
|
||||
return (ValueType)((Any::Holder<long double>*)(operand->mContent))->mValue;
|
||||
}
|
||||
else if (IsInt8Type(t2))
|
||||
{
|
||||
return (ValueType)((Any::Holder<int8_t>*)(operand->mContent))->mValue;
|
||||
}
|
||||
else if (IsUint8Type(t2))
|
||||
{
|
||||
return (ValueType)((Any::Holder<uint8_t>*)(operand->mContent))->mValue;
|
||||
}
|
||||
else if (IsInt16Type(t2))
|
||||
{
|
||||
return (ValueType)((Any::Holder<int16_t>*)(operand->mContent))->mValue;
|
||||
}
|
||||
else if (IsUint16Type(t2))
|
||||
{
|
||||
return (ValueType)((Any::Holder<uint16_t>*)(operand->mContent))->mValue;
|
||||
}
|
||||
else if (IsIntType(t2))
|
||||
{
|
||||
return (ValueType)((Any::Holder<int32_t>*)(operand->mContent))->mValue;
|
||||
}
|
||||
else if (IsUintType(t2))
|
||||
{
|
||||
return (ValueType)((Any::Holder<uint32_t>*)(operand->mContent))->mValue;
|
||||
}
|
||||
else if (IsInt64Type(t2))
|
||||
{
|
||||
return (ValueType)((Any::Holder<int64_t>*)(operand->mContent))->mValue;
|
||||
}
|
||||
else if (IsUint64Type(t2))
|
||||
{
|
||||
return (ValueType)((Any::Holder<uint64_t>*)(operand->mContent))->mValue;
|
||||
}
|
||||
else
|
||||
{
|
||||
UT_THROW(BadCastException, std::string("AnyNumberCast error. unknown number type:", t2.name()));
|
||||
}
|
||||
}
|
||||
|
||||
UT_THROW(BadCastException, std::string("AnyNumberCast error. not number type"));
|
||||
}
|
||||
|
||||
template<typename ValueType>
|
||||
ValueType AnyNumberCast(const Any& operand)
|
||||
{
|
||||
return AnyNumberCast<ValueType>(&operand);
|
||||
}
|
||||
|
||||
static inline const std::string& ToString(const Any& operand)
|
||||
{
|
||||
if (operand.Empty())
|
||||
{
|
||||
return UT_EMPTY_STR;
|
||||
}
|
||||
|
||||
return AnyCast<std::string>(operand);
|
||||
}
|
||||
|
||||
static inline void StringTo(const std::string& s, Any& value)
|
||||
{
|
||||
value = s;
|
||||
}
|
||||
|
||||
static inline void StringTo(const char* s, Any& value)
|
||||
{
|
||||
value = std::string(s);
|
||||
}
|
||||
|
||||
static inline void StringTo(const char* s, size_t len, Any& value)
|
||||
{
|
||||
value = std::string(s, len);
|
||||
}
|
||||
|
||||
static inline void StringTo(const char* s, size_t pos, size_t len, Any& value)
|
||||
{
|
||||
value = std::string(s, pos, len);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
#endif//__UT_ANY_HPP__
|
||||
|
|
@ -1,149 +0,0 @@
|
|||
#ifndef __UT_ASSERT_HPP__
|
||||
#define __UT_ASSERT_HPP__
|
||||
|
||||
#include <unitree/common/decl.hpp>
|
||||
|
||||
/*
|
||||
* Declare assert output
|
||||
*/
|
||||
#define UT_ASSERT_OUT(debug, file, func, line, r) \
|
||||
if (debug) \
|
||||
{ \
|
||||
std::cout << "[" << ::time(NULL) \
|
||||
<< "] [" << ::gettid() \
|
||||
<< "] UT_ASSERT DEBUG at __FILE__:" << file \
|
||||
<< ", __FUNCTION__:" << func \
|
||||
<< ", __LINE__:" << line \
|
||||
<< ", r:" << r \
|
||||
<< ", errno:" << errno \
|
||||
<< std::endl; \
|
||||
} \
|
||||
else \
|
||||
{ \
|
||||
std::cout << "[" << ::time(NULL) \
|
||||
<< "] [" << ::gettid() \
|
||||
<< "] UT_ASSERT ABORT at __FILE__:" << file \
|
||||
<< ", __FUNCTION__:" << func \
|
||||
<< ", __LINE__:" << line \
|
||||
<< ", r:" << r \
|
||||
<< ", errno:" << errno \
|
||||
<< std::endl; \
|
||||
}
|
||||
|
||||
#define UT_ASSERT_ABORT(debug, file, func, line, r) \
|
||||
if (debug) \
|
||||
{ \
|
||||
UT_ASSERT_OUT(1, file, func, line, r); \
|
||||
} \
|
||||
else \
|
||||
{ \
|
||||
UT_ASSERT_OUT(0, file, func, line, r); \
|
||||
abort(); \
|
||||
}
|
||||
|
||||
/*
|
||||
* Declare assert return value
|
||||
*/
|
||||
#define UT_ASSERT_EQ(x, r) \
|
||||
unitree::common::AssertEqual(x, r, 0, __FILE__, \
|
||||
__PRETTY_FUNCTION__, __LINE__)
|
||||
|
||||
#define UT_ASSERT_EQ_DEBUG(x, r) \
|
||||
unitree::common::AssertEqual(x, r, 1, __FILE__, \
|
||||
__PRETTY_FUNCTION__, __LINE__)
|
||||
|
||||
#define UT_ASSERT_NOT_EQ(x, r) \
|
||||
unitree::common::AssertNotEqual(x, r, 0, __FILE__, \
|
||||
__PRETTY_FUNCTION__, __LINE__)
|
||||
|
||||
#define UT_ASSERT_NOT_EQ_DEBUG(x, r) \
|
||||
unitree::common::AssertNotEqual(x, r, 1, __FILE__, \
|
||||
__PRETTY_FUNCTION__, __LINE__)
|
||||
|
||||
/*
|
||||
* Declare assert return value and errno
|
||||
*/
|
||||
#define UT_ASSERT_ENO_EQ(x, r, eno) \
|
||||
unitree::common::AssertEqual(x, r, eno, 0, __FILE__, \
|
||||
__PRETTY_FUNCTION__, __LINE__)
|
||||
|
||||
#define UT_ASSERT_ENO_EQ_DEBUG(x, r, eno) \
|
||||
unitree::common::AssertEqual(x, r, eno, 1, __FILE__, \
|
||||
__PRETTY_FUNCTION__, __LINE__)
|
||||
|
||||
#define UT_ASSERT_ENO_EQ_EX(x, r, eno) \
|
||||
unitree::common::AssertEqualEx(x, r, eno, 0, __FILE__, \
|
||||
__PRETTY_FUNCTION__, __LINE__)
|
||||
|
||||
#define UT_ASSERT_ENO_EQ_EX_DEBUG(x, r, eno) \
|
||||
unitree::common::AssertEqualEx(x, r, eno, 1, __FILE__, \
|
||||
__PRETTY_FUNCTION__, __LINE__)
|
||||
|
||||
/*
|
||||
* Declare assert wrapper
|
||||
*/
|
||||
#define UT_ASSERT_0(x) \
|
||||
UT_ASSERT_EQ(x, 0)
|
||||
|
||||
#define UT_ASSERT_DEBUG_0(x) \
|
||||
UT_ASSERT_EQ_DEBUG(x, 0)
|
||||
|
||||
#define UT_ASSERT_ENO_0(x, eno) \
|
||||
UT_ASSERT_ENO_EQ(x, 0, eno)
|
||||
|
||||
#define UT_ASSERT_ENO_DEBUG_0(x, eno) \
|
||||
UT_ASSERT_ENO_EQ_DEBUG(x, 0, eno)
|
||||
|
||||
|
||||
//Declare assert function
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
inline int AssertEqual(int r, int expectRet, bool debug,
|
||||
const char* file, const char* func, int line)
|
||||
{
|
||||
if (UT_UNLIKELY(r != expectRet))
|
||||
{
|
||||
UT_ASSERT_ABORT(debug, file, func, line, r);
|
||||
}
|
||||
|
||||
return r;
|
||||
}
|
||||
|
||||
inline int AssertNotEqual(int r, int expectRet, bool debug,
|
||||
const char* file, const char* func, int line)
|
||||
{
|
||||
if (UT_UNLIKELY(r == expectRet))
|
||||
{
|
||||
UT_ASSERT_ABORT(debug, file, func, line, r);
|
||||
}
|
||||
|
||||
return r;
|
||||
}
|
||||
|
||||
inline int AssertEqual(int r, int expectRet, int expectErrno, bool debug,
|
||||
const char* file, const char* func, int line)
|
||||
{
|
||||
if (UT_UNLIKELY(r != expectRet) && UT_UNLIKELY(errno != expectErrno))
|
||||
{
|
||||
UT_ASSERT_ABORT(debug, file, func, line, r);
|
||||
}
|
||||
|
||||
return r;
|
||||
}
|
||||
|
||||
inline int AssertEqualEx(int r, int expectRet, int expectErrno, bool debug,
|
||||
const char* file, const char* func, int line)
|
||||
{
|
||||
if (UT_UNLIKELY(r != 0) && UT_UNLIKELY(r != expectRet) &&
|
||||
UT_UNLIKELY(errno != expectErrno))
|
||||
{
|
||||
UT_ASSERT_ABORT(debug, file, func, line, r);
|
||||
}
|
||||
|
||||
return r;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif//__UT_ASSERT_HPP__
|
||||
|
|
@ -1,137 +0,0 @@
|
|||
#ifndef __UT_BLOCK_QUEUE_HPP__
|
||||
#define __UT_BLOCK_QUEUE_HPP__
|
||||
|
||||
#include <unitree/common/exception.hpp>
|
||||
#include <unitree/common/lock/lock.hpp>
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
template<typename T>
|
||||
class BlockQueue
|
||||
{
|
||||
public:
|
||||
BlockQueue(uint64_t maxSize = UT_QUEUE_MAX_LEN) :
|
||||
mMaxSize(maxSize), mCurSize(0)
|
||||
{
|
||||
if (mMaxSize == 0)
|
||||
{
|
||||
mMaxSize = UT_QUEUE_MAX_LEN;
|
||||
}
|
||||
}
|
||||
|
||||
bool Put(const T& t, bool replace = false, bool putfront = false)
|
||||
{
|
||||
/*
|
||||
* if queue is full or full-replaced occured return false
|
||||
*/
|
||||
bool noneReplaced = true;
|
||||
|
||||
LockGuard<MutexCond> guard(mMutexCond);
|
||||
if (mCurSize >= mMaxSize)
|
||||
{
|
||||
if (!replace)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
noneReplaced = false;
|
||||
|
||||
mQueue.pop_front();
|
||||
mCurSize --;
|
||||
}
|
||||
|
||||
if (putfront)
|
||||
{
|
||||
mQueue.emplace_front(t);
|
||||
}
|
||||
else
|
||||
{
|
||||
mQueue.emplace_back(t);
|
||||
}
|
||||
|
||||
mCurSize ++;
|
||||
mMutexCond.Notify();
|
||||
|
||||
return noneReplaced;
|
||||
}
|
||||
|
||||
bool Get(T& t, uint64_t microsec = 0)
|
||||
{
|
||||
LockGuard<MutexCond> guard(mMutexCond);
|
||||
return GetTimeout(t, microsec);
|
||||
}
|
||||
|
||||
T Get(uint64_t microsec = 0)
|
||||
{
|
||||
LockGuard<MutexCond> guard(mMutexCond);
|
||||
T t;
|
||||
if (GetTimeout(t, microsec))
|
||||
{
|
||||
return std::move(t);
|
||||
}
|
||||
|
||||
UT_THROW(TimeoutException, "block queue get timeout or interrupted");
|
||||
}
|
||||
|
||||
bool Empty()
|
||||
{
|
||||
return mCurSize == 0;
|
||||
}
|
||||
|
||||
uint64_t Size()
|
||||
{
|
||||
return mCurSize;
|
||||
}
|
||||
|
||||
void Interrupt(bool all = false)
|
||||
{
|
||||
LockGuard<MutexCond> guard(mMutexCond);
|
||||
if (all)
|
||||
{
|
||||
mMutexCond.NotifyAll();
|
||||
}
|
||||
else
|
||||
{
|
||||
mMutexCond.Notify();
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
bool GetTimeout(T& t, uint64_t microsec = 0)
|
||||
{
|
||||
if (mQueue.empty())
|
||||
{
|
||||
if (!mMutexCond.Wait(microsec))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (mQueue.empty())
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
t = mQueue.front();
|
||||
mQueue.pop_front();
|
||||
|
||||
mCurSize--;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
private:
|
||||
uint64_t mMaxSize;
|
||||
uint64_t mCurSize;
|
||||
std::list<T> mQueue;
|
||||
MutexCond mMutexCond;
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
using BlockQueuePtr = std::shared_ptr<BlockQueue<T>>;
|
||||
|
||||
}
|
||||
}
|
||||
#endif//__UT_BLOCK_QUEUE_HPP__
|
||||
|
|
@ -1,35 +0,0 @@
|
|||
#ifndef __UT_DDS_CALLBACK_HPP__
|
||||
#define __UT_DDS_CALLBACK_HPP__
|
||||
|
||||
#include <unitree/common/decl.hpp>
|
||||
|
||||
using DdsMessageHandler = std::function<void(const void*)>;
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
class DdsReaderCallback
|
||||
{
|
||||
public:
|
||||
DdsReaderCallback();
|
||||
DdsReaderCallback(const DdsMessageHandler& handler);
|
||||
DdsReaderCallback(const DdsReaderCallback& cb);
|
||||
DdsReaderCallback& operator=(const DdsReaderCallback& cb);
|
||||
|
||||
~DdsReaderCallback();
|
||||
|
||||
public:
|
||||
bool HasMessageHandler() const;
|
||||
void OnDataAvailable(const void* message);
|
||||
|
||||
private:
|
||||
DdsMessageHandler mMessageHandler;
|
||||
};
|
||||
|
||||
using DdsReaderCallbackPtr = std::shared_ptr<DdsReaderCallback>;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
#endif//__UT_DDS_CALLBACK_HPP__
|
||||
|
|
@ -1,148 +0,0 @@
|
|||
#ifndef __DDS_EASY_MODEL_HPP__
|
||||
#define __DDS_EASY_MODEL_HPP__
|
||||
|
||||
#include <unitree/common/dds/dds_topic_channel.hpp>
|
||||
#include <unitree/common/dds/dds_parameter.hpp>
|
||||
#include <unitree/common/dds/dds_qos_realize.hpp>
|
||||
#include <unitree/common/string_tool.hpp>
|
||||
|
||||
#define UT_DDS_PARAMETER_CONFIG_FILENAME "dds_parameter.json"
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
class DdsEasyModel
|
||||
{
|
||||
public:
|
||||
explicit DdsEasyModel();
|
||||
~DdsEasyModel();
|
||||
|
||||
void Init(uint32_t domainId);
|
||||
void Init(const std::string& ddsParameterFileName = "");
|
||||
void Init(const JsonMap& param);
|
||||
|
||||
template<typename MSG>
|
||||
void SetTopic(const std::string& topic)
|
||||
{
|
||||
DdsTopicChannelPtr<MSG> channel = GetChannel<MSG>(topic);
|
||||
if (!channel)
|
||||
{
|
||||
channel = DdsTopicChannelPtr<MSG>(new DdsTopicChannel<MSG>());
|
||||
mChannelMap[topic] = std::static_pointer_cast<DdsTopicChannelAbstract>(channel);
|
||||
|
||||
DdsTopicQos topicQos;
|
||||
GetTopicQos(topic, topicQos);
|
||||
channel->SetTopic(mParticipant, topic, topicQos);
|
||||
}
|
||||
|
||||
DdsWriterPtr<MSG> writer = channel->GetWriter();
|
||||
if (!writer)
|
||||
{
|
||||
DdsWriterQos writerQos;
|
||||
GetWriterQos(topic, writerQos);
|
||||
channel->SetWriter(GetPublisher(topic), writerQos);
|
||||
}
|
||||
else
|
||||
{
|
||||
UT_THROW(CommonException, std::string("topic reader is already exist. topic:") + topic);
|
||||
}
|
||||
}
|
||||
|
||||
template<typename MSG>
|
||||
void SetTopic(const std::string& topic, const DdsMessageHandler& handler, int32_t queuelen = 0)
|
||||
{
|
||||
DdsReaderCallback cb(handler);
|
||||
SetTopic<MSG>(topic, cb, queuelen);
|
||||
}
|
||||
|
||||
template<typename MSG>
|
||||
void SetTopic(const std::string& topic, const DdsReaderCallback& rcb, int32_t queuelen = 0)
|
||||
{
|
||||
DdsTopicChannelPtr<MSG> channel = GetChannel<MSG>(topic);
|
||||
if (!channel)
|
||||
{
|
||||
channel = DdsTopicChannelPtr<MSG>(new DdsTopicChannel<MSG>());
|
||||
mChannelMap[topic] = std::static_pointer_cast<DdsTopicChannelAbstract>(channel);
|
||||
|
||||
DdsTopicQos topicQos;
|
||||
GetTopicQos(topic, topicQos);
|
||||
channel->SetTopic(mParticipant, topic, topicQos);
|
||||
}
|
||||
|
||||
DdsReaderPtr<MSG> reader = channel->GetReader();
|
||||
if (!reader)
|
||||
{
|
||||
DdsReaderQos readerQos;
|
||||
GetReaderQos(topic, readerQos);
|
||||
channel->SetReader(GetSubscriber(topic), readerQos, rcb, queuelen);
|
||||
}
|
||||
else
|
||||
{
|
||||
UT_THROW(CommonException, std::string("topic reader is already exist. topic:") + topic);
|
||||
}
|
||||
}
|
||||
|
||||
template<typename MSG>
|
||||
bool WriteMessage(const std::string topic, const MSG& message, int64_t waitMicrosec = 0)
|
||||
{
|
||||
DdsTopicChannelPtr<MSG> channel = GetChannel<MSG>(topic);
|
||||
if (channel == NULL)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
return channel->Write(message, waitMicrosec);
|
||||
}
|
||||
|
||||
bool WriteMessage(const std::string topic, const void* message, int64_t waitMicrosec = 0);
|
||||
|
||||
int64_t GetLastDataAvailableTime(const std::string topic);
|
||||
|
||||
private:
|
||||
void GetTopicQos(const std::string& topic, DdsTopicQos& qos);
|
||||
void GetWriterQos(const std::string& topic, DdsWriterQos& qos);
|
||||
void GetReaderQos(const std::string& topic, DdsReaderQos& qos);
|
||||
|
||||
DdsTopicChannelAbstractPtr GetChannel(const std::string& topic);
|
||||
|
||||
template<typename MSG>
|
||||
DdsTopicChannelPtr<MSG> GetChannel(const std::string& topic)
|
||||
{
|
||||
DdsTopicChannelPtr<MSG> channel;
|
||||
|
||||
DdsTopicChannelAbstractPtr channelAbstract = GetChannel(topic);
|
||||
if (channelAbstract)
|
||||
{
|
||||
channel = std::static_pointer_cast<DdsTopicChannel<MSG>>(channelAbstract);
|
||||
}
|
||||
|
||||
return channel;
|
||||
}
|
||||
|
||||
DdsSubscriberPtr GetSubscriber(const std::string& topic);
|
||||
DdsSubscriberPtr GetSubscriberDefault();
|
||||
|
||||
DdsPublisherPtr GetPublisher(const std::string& topic);
|
||||
DdsPublisherPtr GetPublisherDefault();
|
||||
|
||||
private:
|
||||
DdsParameter mDdsParameter;
|
||||
|
||||
DdsParticipantPtr mParticipant;
|
||||
std::vector<DdsPublisherPtr> mPublisherList;
|
||||
std::vector<DdsSubscriberPtr> mSubscriberList;
|
||||
DdsPublisherPtr mPublisherDefault;
|
||||
DdsSubscriberPtr mSubscriberDefault;
|
||||
|
||||
std::map<std::string,DdsTopicChannelAbstractPtr> mChannelMap;
|
||||
|
||||
Logger *mLogger;
|
||||
};
|
||||
|
||||
using DdsEasyModelPtr = std::shared_ptr<DdsEasyModel>;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
#endif//__DDS_EASY_MODEL_HPP__
|
||||
|
|
@ -1,406 +0,0 @@
|
|||
#ifndef __UT_DDS_ENTITY_HPP__
|
||||
#define __UT_DDS_ENTITY_HPP__
|
||||
|
||||
#include <dds/dds.hpp>
|
||||
#include <unitree/common/log/log.hpp>
|
||||
#include <unitree/common/block_queue.hpp>
|
||||
#include <unitree/common/thread/thread.hpp>
|
||||
#include <unitree/common/time/time_tool.hpp>
|
||||
#include <unitree/common/time/sleep.hpp>
|
||||
#include <unitree/common/dds/dds_exception.hpp>
|
||||
#include <unitree/common/dds/dds_callback.hpp>
|
||||
#include <unitree/common/dds/dds_qos.hpp>
|
||||
#include <unitree/common/dds/dds_traits.hpp>
|
||||
|
||||
#define __UT_DDS_NULL__ ::dds::core::null
|
||||
|
||||
/*
|
||||
* dds wait sub/pub matched default time slice.
|
||||
* default 50000 us
|
||||
*/
|
||||
#define __UT_DDS_WAIT_MATCHED_TIME_SLICE 50000
|
||||
|
||||
using namespace org::eclipse::cyclonedds;
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
class DdsLogger
|
||||
{
|
||||
public:
|
||||
DdsLogger();
|
||||
virtual ~DdsLogger();
|
||||
|
||||
protected:
|
||||
Logger* mLogger;
|
||||
};
|
||||
|
||||
/*
|
||||
* @brief: DdsParticipant
|
||||
*/
|
||||
class DdsParticipant : public DdsLogger
|
||||
{
|
||||
public:
|
||||
using NATIVE_TYPE = ::dds::domain::DomainParticipant;
|
||||
|
||||
explicit DdsParticipant(uint32_t domainId, const DdsParticipantQos& qos, const std::string& config = "");
|
||||
~DdsParticipant();
|
||||
|
||||
const NATIVE_TYPE& GetNative() const;
|
||||
|
||||
private:
|
||||
NATIVE_TYPE mNative;
|
||||
};
|
||||
|
||||
using DdsParticipantPtr = std::shared_ptr<DdsParticipant>;
|
||||
|
||||
|
||||
/*
|
||||
* @brief: DdsPublisher
|
||||
*/
|
||||
class DdsPublisher : public DdsLogger
|
||||
{
|
||||
public:
|
||||
using NATIVE_TYPE = ::dds::pub::Publisher;
|
||||
|
||||
explicit DdsPublisher(const DdsParticipantPtr& participant, const DdsPublisherQos& qos);
|
||||
~DdsPublisher();
|
||||
|
||||
const NATIVE_TYPE& GetNative() const;
|
||||
|
||||
private:
|
||||
NATIVE_TYPE mNative;
|
||||
};
|
||||
|
||||
using DdsPublisherPtr = std::shared_ptr<DdsPublisher>;
|
||||
|
||||
|
||||
/*
|
||||
* @brief: DdsSubscriber
|
||||
*/
|
||||
class DdsSubscriber : public DdsLogger
|
||||
{
|
||||
public:
|
||||
using NATIVE_TYPE = ::dds::sub::Subscriber;
|
||||
|
||||
explicit DdsSubscriber(const DdsParticipantPtr& participant, const DdsSubscriberQos& qos);
|
||||
~DdsSubscriber();
|
||||
|
||||
const NATIVE_TYPE& GetNative() const;
|
||||
|
||||
private:
|
||||
NATIVE_TYPE mNative;
|
||||
};
|
||||
|
||||
using DdsSubscriberPtr = std::shared_ptr<DdsSubscriber>;
|
||||
|
||||
|
||||
/*
|
||||
* @brief: DdsTopic
|
||||
*/
|
||||
template<typename MSG>
|
||||
class DdsTopic : public DdsLogger
|
||||
{
|
||||
public:
|
||||
using NATIVE_TYPE = ::dds::topic::Topic<MSG>;
|
||||
|
||||
explicit DdsTopic(const DdsParticipantPtr& participant, const std::string& name, const DdsTopicQos& qos) :
|
||||
mNative(__UT_DDS_NULL__)
|
||||
{
|
||||
UT_DDS_EXCEPTION_TRY
|
||||
|
||||
auto topicQos = participant->GetNative().default_topic_qos();
|
||||
qos.CopyToNativeQos(topicQos);
|
||||
|
||||
mNative = NATIVE_TYPE(participant->GetNative(), name, topicQos);
|
||||
|
||||
UT_DDS_EXCEPTION_CATCH(mLogger, true)
|
||||
}
|
||||
|
||||
~DdsTopic()
|
||||
{
|
||||
mNative = __UT_DDS_NULL__;
|
||||
}
|
||||
|
||||
const NATIVE_TYPE& GetNative() const
|
||||
{
|
||||
return mNative;
|
||||
}
|
||||
|
||||
private:
|
||||
NATIVE_TYPE mNative;
|
||||
};
|
||||
|
||||
template<typename MSG>
|
||||
using DdsTopicPtr = std::shared_ptr<DdsTopic<MSG>>;
|
||||
|
||||
|
||||
/*
|
||||
* @brief: DdsWriter
|
||||
*/
|
||||
template<typename MSG>
|
||||
class DdsWriter : public DdsLogger
|
||||
{
|
||||
public:
|
||||
using NATIVE_TYPE = ::dds::pub::DataWriter<MSG>;
|
||||
|
||||
explicit DdsWriter(const DdsPublisherPtr publisher, const DdsTopicPtr<MSG>& topic, const DdsWriterQos& qos) :
|
||||
mNative(__UT_DDS_NULL__)
|
||||
{
|
||||
UT_DDS_EXCEPTION_TRY
|
||||
|
||||
auto writerQos = publisher->GetNative().default_datawriter_qos();
|
||||
qos.CopyToNativeQos(writerQos);
|
||||
|
||||
mNative = NATIVE_TYPE(publisher->GetNative(), topic->GetNative(), writerQos);
|
||||
|
||||
UT_DDS_EXCEPTION_CATCH(mLogger, true)
|
||||
}
|
||||
|
||||
~DdsWriter()
|
||||
{
|
||||
mNative = __UT_DDS_NULL__;
|
||||
}
|
||||
|
||||
const NATIVE_TYPE& GetNative() const
|
||||
{
|
||||
return mNative;
|
||||
}
|
||||
|
||||
bool Write(const MSG& message, int64_t waitMicrosec)
|
||||
{
|
||||
if (waitMicrosec > 0 && !WaitReader(waitMicrosec))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
UT_DDS_EXCEPTION_TRY
|
||||
{
|
||||
mNative.write(message);
|
||||
return true;
|
||||
}
|
||||
UT_DDS_EXCEPTION_CATCH(mLogger, false)
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
private:
|
||||
bool WaitReader(int64_t waitMicrosec)
|
||||
{
|
||||
while (mNative.publication_matched_status().current_count() == 0)
|
||||
{
|
||||
if (waitMicrosec <= 0)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
MicroSleep(__UT_DDS_WAIT_MATCHED_TIME_SLICE);
|
||||
waitMicrosec -=__UT_DDS_WAIT_MATCHED_TIME_SLICE;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
private:
|
||||
NATIVE_TYPE mNative;
|
||||
};
|
||||
|
||||
template<typename MSG>
|
||||
using DdsWriterPtr = std::shared_ptr<DdsWriter<MSG>>;
|
||||
|
||||
|
||||
/*
|
||||
* @brief: DdsReaderListener
|
||||
*/
|
||||
template<typename MSG>
|
||||
class DdsReaderListener : public ::dds::sub::NoOpDataReaderListener<MSG>, DdsLogger
|
||||
{
|
||||
public:
|
||||
using NATIVE_TYPE = ::dds::sub::DataReaderListener<MSG>;
|
||||
using MSG_PTR = std::shared_ptr<MSG>;
|
||||
|
||||
explicit DdsReaderListener() :
|
||||
mHasQueue(false), mQuit(false), mMask(::dds::core::status::StatusMask::none()), mLastDataAvailableTime(0)
|
||||
{}
|
||||
|
||||
~DdsReaderListener()
|
||||
{
|
||||
if (mHasQueue)
|
||||
{
|
||||
mQuit = true;
|
||||
mDataQueuePtr->Interrupt(false);
|
||||
mDataQueueThreadPtr->Wait();
|
||||
}
|
||||
}
|
||||
|
||||
void SetCallback(const DdsReaderCallback& cb)
|
||||
{
|
||||
if (cb.HasMessageHandler())
|
||||
{
|
||||
mMask |= ::dds::core::status::StatusMask::data_available();
|
||||
}
|
||||
|
||||
mCallbackPtr.reset(new DdsReaderCallback(cb));
|
||||
}
|
||||
|
||||
void SetQueue(int32_t len)
|
||||
{
|
||||
if (len <= 0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
mHasQueue = true;
|
||||
mDataQueuePtr.reset(new BlockQueue<MSG_PTR>(len));
|
||||
|
||||
auto queueThreadFunc = [this]() {
|
||||
while (true)
|
||||
{
|
||||
if (mCallbackPtr && mCallbackPtr->HasMessageHandler())
|
||||
{
|
||||
break;
|
||||
}
|
||||
else
|
||||
{
|
||||
MicroSleep(__UT_DDS_WAIT_MATCHED_TIME_SLICE);
|
||||
}
|
||||
}
|
||||
while (!mQuit)
|
||||
{
|
||||
MSG_PTR dataPtr;
|
||||
if (mDataQueuePtr->Get(dataPtr))
|
||||
{
|
||||
if (dataPtr)
|
||||
{
|
||||
mCallbackPtr->OnDataAvailable(dataPtr.get());
|
||||
}
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
};
|
||||
|
||||
mDataQueueThreadPtr = CreateThreadEx("rlsnr", UT_CPU_ID_NONE, queueThreadFunc);
|
||||
}
|
||||
|
||||
int64_t GetLastDataAvailableTime() const
|
||||
{
|
||||
return mLastDataAvailableTime;
|
||||
}
|
||||
|
||||
NATIVE_TYPE* GetNative() const
|
||||
{
|
||||
return (NATIVE_TYPE*)this;
|
||||
}
|
||||
|
||||
const ::dds::core::status::StatusMask& GetStatusMask() const
|
||||
{
|
||||
return mMask;
|
||||
}
|
||||
|
||||
private:
|
||||
void on_data_available(::dds::sub::DataReader<MSG>& reader)
|
||||
{
|
||||
::dds::sub::LoanedSamples<MSG> samples;
|
||||
samples = reader.take();
|
||||
|
||||
if (samples.length() <= 0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
typename ::dds::sub::LoanedSamples<MSG>::const_iterator iter;
|
||||
for (iter=samples.begin(); iter<samples.end(); ++iter)
|
||||
{
|
||||
const MSG& m = iter->data();
|
||||
if (iter->info().valid())
|
||||
{
|
||||
mLastDataAvailableTime = GetCurrentMonotonicTimeNanosecond();
|
||||
|
||||
if (mHasQueue)
|
||||
{
|
||||
if (!mDataQueuePtr->Put(MSG_PTR(new MSG(m)), true))
|
||||
{
|
||||
LOG_WARNING(mLogger, "earliest mesage was evicted. type:", DdsGetTypeName(MSG));
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
mCallbackPtr->OnDataAvailable((const void*)&m);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
bool mHasQueue;
|
||||
volatile bool mQuit;
|
||||
|
||||
::dds::core::status::StatusMask mMask;
|
||||
int64_t mLastDataAvailableTime;
|
||||
|
||||
DdsReaderCallbackPtr mCallbackPtr;
|
||||
BlockQueuePtr<MSG_PTR> mDataQueuePtr;
|
||||
ThreadPtr mDataQueueThreadPtr;
|
||||
};
|
||||
|
||||
template<typename MSG>
|
||||
using DdsReaderListenerPtr = std::shared_ptr<DdsReaderListener<MSG>>;
|
||||
|
||||
|
||||
/*
|
||||
* @brief: DdsReader
|
||||
*/
|
||||
template<typename MSG>
|
||||
class DdsReader : public DdsLogger
|
||||
{
|
||||
public:
|
||||
using NATIVE_TYPE = ::dds::sub::DataReader<MSG>;
|
||||
|
||||
explicit DdsReader(const DdsSubscriberPtr& subscriber, const DdsTopicPtr<MSG>& topic, const DdsReaderQos& qos) :
|
||||
mNative(__UT_DDS_NULL__)
|
||||
{
|
||||
UT_DDS_EXCEPTION_TRY
|
||||
|
||||
auto readerQos = subscriber->GetNative().default_datareader_qos();
|
||||
qos.CopyToNativeQos(readerQos);
|
||||
|
||||
mNative = NATIVE_TYPE(subscriber->GetNative(), topic->GetNative(), readerQos);
|
||||
|
||||
UT_DDS_EXCEPTION_CATCH(mLogger, true)
|
||||
}
|
||||
|
||||
~DdsReader()
|
||||
{
|
||||
mNative = __UT_DDS_NULL__;
|
||||
}
|
||||
|
||||
const NATIVE_TYPE& GetNative() const
|
||||
{
|
||||
return mNative;
|
||||
}
|
||||
|
||||
void SetListener(const DdsReaderCallback& cb, int32_t qlen)
|
||||
{
|
||||
mListener.SetCallback(cb);
|
||||
mListener.SetQueue(qlen);
|
||||
mNative.listener(mListener.GetNative(), mListener.GetStatusMask());
|
||||
}
|
||||
|
||||
int64_t GetLastDataAvailableTime() const
|
||||
{
|
||||
return mListener.GetLastDataAvailableTime();
|
||||
}
|
||||
|
||||
private:
|
||||
NATIVE_TYPE mNative;
|
||||
DdsReaderListener<MSG> mListener;
|
||||
};
|
||||
|
||||
template<typename MSG>
|
||||
using DdsReaderPtr = std::shared_ptr<DdsReader<MSG>>;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
#endif//__UT_DDS_ENTITY_HPP__
|
||||
|
|
@ -1,17 +0,0 @@
|
|||
#ifndef __UT_DDS_ERROR_HPP__
|
||||
#define __UT_DDS_ERROR_HPP__
|
||||
|
||||
#include <unitree/common/decl.hpp>
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
/*
|
||||
* dds error.
|
||||
*/
|
||||
UT_DECL_ERR(UT_ERR_DDS, 2001, "dds error")
|
||||
|
||||
}
|
||||
}
|
||||
#endif//__UT_DDS_ERROR_HPP__
|
||||
|
|
@ -1,58 +0,0 @@
|
|||
#ifndef __UT_DDS_EXCEPTION_HPP__
|
||||
#define __UT_DDS_EXCEPTION_HPP__
|
||||
|
||||
#include <unitree/common/exception.hpp>
|
||||
#include <unitree/common/dds/dds_error.hpp>
|
||||
|
||||
#define __UT_DDS_EXCEPTION_MESSAGE(e, d) \
|
||||
std::string("Catch dds::core exception. Class:") + __UT_STR(d) + ", Message:" + e.what();
|
||||
|
||||
#define __UT_DDS_EXCEPTION_CATCH(except, l, t) \
|
||||
catch (const except & e) \
|
||||
{ \
|
||||
if (l || t) \
|
||||
{ \
|
||||
std::string __t9b78e5r = __UT_DDS_EXCEPTION_MESSAGE(e, except); \
|
||||
if (l) \
|
||||
{ \
|
||||
LOG_ERROR(l, __t9b78e5r); \
|
||||
} \
|
||||
if (t) \
|
||||
{ \
|
||||
UT_THROW(DdsException, __t9b78e5r); \
|
||||
} \
|
||||
} \
|
||||
}
|
||||
|
||||
#define UT_DDS_EXCEPTION_TRY \
|
||||
try \
|
||||
{
|
||||
|
||||
#define UT_DDS_EXCEPTION_CATCH(l, t) \
|
||||
} \
|
||||
__UT_DDS_EXCEPTION_CATCH(::dds::core::Error, l, t) \
|
||||
__UT_DDS_EXCEPTION_CATCH(::dds::core::InvalidArgumentError, l, t) \
|
||||
__UT_DDS_EXCEPTION_CATCH(::dds::core::TimeoutError, l, t) \
|
||||
__UT_DDS_EXCEPTION_CATCH(::dds::core::UnsupportedError, l, t) \
|
||||
__UT_DDS_EXCEPTION_CATCH(::dds::core::AlreadyClosedError, l, t) \
|
||||
__UT_DDS_EXCEPTION_CATCH(::dds::core::IllegalOperationError, l, t) \
|
||||
__UT_DDS_EXCEPTION_CATCH(::dds::core::NotEnabledError, l, t) \
|
||||
__UT_DDS_EXCEPTION_CATCH(::dds::core::PreconditionNotMetError, l, t) \
|
||||
__UT_DDS_EXCEPTION_CATCH(::dds::core::ImmutablePolicyError, l, t) \
|
||||
__UT_DDS_EXCEPTION_CATCH(::dds::core::InconsistentPolicyError, l, t) \
|
||||
__UT_DDS_EXCEPTION_CATCH(::dds::core::OutOfResourcesError, l, t) \
|
||||
__UT_DDS_EXCEPTION_CATCH(::dds::core::InvalidDowncastError, l, t) \
|
||||
__UT_DDS_EXCEPTION_CATCH(::dds::core::NullReferenceError, l, t) \
|
||||
__UT_DDS_EXCEPTION_CATCH(::dds::core::InvalidDataError, l, t) \
|
||||
__UT_DDS_EXCEPTION_CATCH(::dds::core::Exception, l, t) \
|
||||
__UT_DDS_EXCEPTION_CATCH(std::exception, l, t)
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
UT_DECL_EXCEPTION(DdsException, UT_ERR_DDS, UT_DESC_ERR(UT_ERR_DDS))
|
||||
}
|
||||
}
|
||||
|
||||
#endif//__UT_DDS_EXCEPTION_HPP__
|
||||
|
|
@ -1,62 +0,0 @@
|
|||
#ifndef __UT_DDS_FACTORY_MODEL_HPP__
|
||||
#define __UT_DDS_FACTORY_MODEL_HPP__
|
||||
|
||||
#include <unitree/common/dds/dds_parameter.hpp>
|
||||
#include <unitree/common/dds/dds_topic_channel.hpp>
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
class DdsFactoryModel
|
||||
{
|
||||
public:
|
||||
explicit DdsFactoryModel();
|
||||
~DdsFactoryModel();
|
||||
|
||||
void Init(uint32_t domainId, const std::string& ddsConfig = "");
|
||||
void Init(const std::string& ddsParameterFileName = "");
|
||||
void Init(const JsonMap& param);
|
||||
|
||||
template<typename MSG>
|
||||
DdsTopicChannelPtr<MSG> CreateTopicChannel(const std::string& topic)
|
||||
{
|
||||
DdsTopicChannelPtr<MSG> channel = DdsTopicChannelPtr<MSG>(new DdsTopicChannel<MSG>());
|
||||
channel->SetTopic(mParticipant, topic, mTopicQos);
|
||||
return channel;
|
||||
}
|
||||
|
||||
template<typename MSG>
|
||||
void SetWriter(DdsTopicChannelPtr<MSG>& channelPtr)
|
||||
{
|
||||
channelPtr->SetWriter(mPublisher, mWriterQos);
|
||||
}
|
||||
|
||||
template<typename MSG>
|
||||
void SetReader(DdsTopicChannelPtr<MSG>& channelPtr, const std::function<void(const void*)>& handler, int32_t queuelen = 0)
|
||||
{
|
||||
DdsReaderCallback cb(handler);
|
||||
channelPtr->SetReader(mSubscriber, mReaderQos, cb, queuelen);
|
||||
}
|
||||
|
||||
private:
|
||||
DdsParticipantPtr mParticipant;
|
||||
DdsPublisherPtr mPublisher;
|
||||
DdsSubscriberPtr mSubscriber;
|
||||
|
||||
DdsParticipantQos mParticipantQos;
|
||||
DdsTopicQos mTopicQos;
|
||||
DdsPublisherQos mPublisherQos;
|
||||
DdsSubscriberQos mSubscriberQos;
|
||||
DdsWriterQos mWriterQos;
|
||||
DdsReaderQos mReaderQos;
|
||||
|
||||
Logger *mLogger;
|
||||
};
|
||||
|
||||
using DdsFactoryModelPtr = std::shared_ptr<DdsFactoryModel>;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
#endif//__UT_DDS_FACTORY_MODEL_HPP__
|
||||
|
|
@ -1,37 +0,0 @@
|
|||
#ifndef __UT_DDS_NATIVE_HPP__
|
||||
#define __UT_DDS_NATIVE_HPP__
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
template<typename NATIVE>
|
||||
class DdsNative
|
||||
{
|
||||
public:
|
||||
using NativeType = NATIVE;
|
||||
|
||||
explicit DdsNative()
|
||||
{}
|
||||
|
||||
virtual ~DdsNative()
|
||||
{}
|
||||
|
||||
void SetNative(const NATIVE& native)
|
||||
{
|
||||
mNative = native;
|
||||
}
|
||||
|
||||
const NATIVE& GetNative() const
|
||||
{
|
||||
return mNative;
|
||||
}
|
||||
|
||||
protected:
|
||||
NATIVE mNative;
|
||||
};
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
#endif//
|
||||
|
|
@ -1,214 +0,0 @@
|
|||
#ifndef __UT_DDS_PARAMETER_HPP__
|
||||
#define __UT_DDS_PARAMETER_HPP__
|
||||
|
||||
#include <unitree/common/decl.hpp>
|
||||
#include <unitree/common/dds/dds_qos_parameter.hpp>
|
||||
|
||||
#define UT_DDS_PARAM_KEY_PARTICIPANT "Participant"
|
||||
#define UT_DDS_PARAM_KEY_DOMAINID "DomainId"
|
||||
#define UT_DDS_PARAM_KEY_CONFIG "Config"
|
||||
#define UT_DDS_PARAM_KEY_NAME "Name"
|
||||
#define UT_DDS_PARAM_KEY_TOPIC "Topic"
|
||||
#define UT_DDS_PARAM_KEY_TOPICNAME "TopicName"
|
||||
#define UT_DDS_PARAM_KEY_PUBLISHER "Publisher"
|
||||
#define UT_DDS_PARAM_KEY_SUBSCRIBER "Subscriber"
|
||||
#define UT_DDS_PARAM_KEY_WRITER "Writer"
|
||||
#define UT_DDS_PARAM_KEY_READER "Reader"
|
||||
#define UT_DDS_PARAM_KEY_QOS "Qos"
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
class DdsQosParameterHolder
|
||||
{
|
||||
public:
|
||||
DdsQosParameterHolder();
|
||||
|
||||
virtual ~DdsQosParameterHolder();
|
||||
|
||||
void SetQos(const DdsQosParameter& qos);
|
||||
const DdsQosParameter& GetQos() const;
|
||||
|
||||
protected:
|
||||
DdsQosParameter mQos;
|
||||
};
|
||||
|
||||
class DdsParticipantParameter : public DdsQosParameterHolder
|
||||
{
|
||||
public:
|
||||
DdsParticipantParameter();
|
||||
DdsParticipantParameter(uint32_t domainId, const std::string& config = "");
|
||||
|
||||
~DdsParticipantParameter();
|
||||
|
||||
void SetDomainId(int32_t domainId);
|
||||
uint32_t GetDomainId() const;
|
||||
|
||||
void SetConfig(const std::string& config);
|
||||
const std::string& GetConfig() const;
|
||||
|
||||
private:
|
||||
uint32_t mDomainId;
|
||||
std::string mConfig;
|
||||
};
|
||||
|
||||
class DdsTopicParameter : public DdsQosParameterHolder
|
||||
{
|
||||
public:
|
||||
DdsTopicParameter();
|
||||
DdsTopicParameter(const std::string& name);
|
||||
|
||||
~DdsTopicParameter();
|
||||
|
||||
void SetName(const std::string& name);
|
||||
const std::string& GetName() const;
|
||||
|
||||
private:
|
||||
std::string mName;
|
||||
};
|
||||
|
||||
class DdsTopicParameterHolder
|
||||
{
|
||||
public:
|
||||
DdsTopicParameterHolder();
|
||||
DdsTopicParameterHolder(const std::string& topicName);
|
||||
|
||||
virtual ~DdsTopicParameterHolder();
|
||||
|
||||
void SetTopicName(const std::string& topicName);
|
||||
const std::string& GetTopicName() const;
|
||||
|
||||
private:
|
||||
std::string mTopicName;
|
||||
};
|
||||
|
||||
class DdsWriterParameter : public DdsTopicParameterHolder, public DdsQosParameterHolder
|
||||
{
|
||||
public:
|
||||
DdsWriterParameter();
|
||||
DdsWriterParameter(const std::string& topicName);
|
||||
|
||||
~DdsWriterParameter();
|
||||
};
|
||||
|
||||
class DdsWriterParameterHolder
|
||||
{
|
||||
public:
|
||||
DdsWriterParameterHolder();
|
||||
|
||||
virtual ~DdsWriterParameterHolder();
|
||||
|
||||
void SetWriter(const DdsWriterParameter& writer);
|
||||
const DdsWriterParameter& GetWriter() const;
|
||||
|
||||
private:
|
||||
DdsWriterParameter mWriter;
|
||||
};
|
||||
|
||||
class DdsReaderParameter : public DdsTopicParameterHolder, public DdsQosParameterHolder
|
||||
{
|
||||
public:
|
||||
DdsReaderParameter();
|
||||
DdsReaderParameter(const std::string& topicName);
|
||||
|
||||
virtual ~DdsReaderParameter();
|
||||
};
|
||||
|
||||
class DdsReaderParameterHolder
|
||||
{
|
||||
public:
|
||||
DdsReaderParameterHolder();
|
||||
|
||||
virtual ~DdsReaderParameterHolder();
|
||||
|
||||
void SetReader(const DdsReaderParameter& reader);
|
||||
const DdsReaderParameter& GetReader() const;
|
||||
|
||||
private:
|
||||
DdsReaderParameter mReader;
|
||||
};
|
||||
|
||||
class DdsPublisherParameter : public DdsQosParameterHolder
|
||||
{
|
||||
public:
|
||||
DdsPublisherParameter();
|
||||
|
||||
~DdsPublisherParameter();
|
||||
|
||||
void AppendWriter(const DdsWriterParameter& writer);
|
||||
void SetWriter(const std::vector<DdsWriterParameter>& writer);
|
||||
const std::vector<DdsWriterParameter>& GetWriter() const;
|
||||
|
||||
private:
|
||||
std::vector<DdsWriterParameter> mWriter;
|
||||
};
|
||||
|
||||
class DdsSubscriberParameter : public DdsQosParameterHolder
|
||||
{
|
||||
public:
|
||||
DdsSubscriberParameter();
|
||||
~DdsSubscriberParameter();
|
||||
|
||||
void AppendReader(const DdsReaderParameter& reader);
|
||||
void SetReader(const std::vector<DdsReaderParameter>& reader);
|
||||
const std::vector<DdsReaderParameter>& GetReader() const;
|
||||
|
||||
private:
|
||||
std::vector<DdsReaderParameter> mReader;
|
||||
};
|
||||
|
||||
class DdsParameter
|
||||
{
|
||||
public:
|
||||
DdsParameter();
|
||||
DdsParameter(const JsonMap& param);
|
||||
|
||||
~DdsParameter();
|
||||
|
||||
void Init(const JsonMap& param);
|
||||
|
||||
uint32_t GetDomainId();
|
||||
const std::string& GetConfig() const;
|
||||
|
||||
const DdsParticipantParameter& GetParticipant();
|
||||
|
||||
void AppendTopic(const DdsTopicParameter& topic);
|
||||
const std::map<std::string,DdsTopicParameter>& GetTopic() const;
|
||||
|
||||
void AppendPublisher(const DdsPublisherParameter& publisher);
|
||||
void SetPublisher(const std::vector<DdsPublisherParameter>& publisher);
|
||||
const std::vector<DdsPublisherParameter>& GetPublisher() const;
|
||||
|
||||
void AppendSubscriber(const DdsSubscriberParameter& subscriber);
|
||||
void SetSubscriber(const std::vector<DdsSubscriberParameter>& subscriber);
|
||||
const std::vector<DdsSubscriberParameter>& GetSubscriber() const;
|
||||
|
||||
const DdsQosParameter& GetParticipantQos() const;
|
||||
const DdsQosParameter& GetTopicQos() const;
|
||||
const DdsQosParameter& GetPublisherQos() const;
|
||||
const DdsQosParameter& GetSubscriberQos() const;
|
||||
const DdsQosParameter& GetWriterQos() const;
|
||||
const DdsQosParameter& GetReaderQos() const;
|
||||
|
||||
private:
|
||||
uint32_t mDomainId;
|
||||
std::string mConfig;
|
||||
|
||||
DdsQosParameter mParticipantQos;
|
||||
DdsQosParameter mTopicQos;
|
||||
DdsQosParameter mPublisherQos;
|
||||
DdsQosParameter mSubscriberQos;
|
||||
DdsQosParameter mWriterQos;
|
||||
DdsQosParameter mReaderQos;
|
||||
|
||||
DdsParticipantParameter mParticipant;
|
||||
std::map<std::string,DdsTopicParameter> mTopic;
|
||||
std::vector<DdsPublisherParameter> mPublisher;
|
||||
std::vector<DdsSubscriberParameter> mSubscriber;
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
}
|
||||
#endif//__UT_DDS_PARAMETER_HPP__
|
||||
|
|
@ -1,71 +0,0 @@
|
|||
#ifndef __UT_DDS_QOS_HPP__
|
||||
#define __UT_DDS_QOS_HPP__
|
||||
|
||||
#include <unitree/common/dds/dds_qos_policy.hpp>
|
||||
|
||||
using namespace org::eclipse::cyclonedds;
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
#define UT_DECL_DDS_QOS(QosType, QosNative) \
|
||||
class QosType: public QosNative \
|
||||
{ \
|
||||
public: \
|
||||
using NativeQosType = QosNative::NativeType; \
|
||||
explicit QosType() \
|
||||
{ \
|
||||
InitPolicyDefault(); \
|
||||
} \
|
||||
template<typename POLICY> \
|
||||
void SetPolicy(const POLICY& policy) \
|
||||
{ \
|
||||
mNative.policy(policy.GetNative()); \
|
||||
mPolicyNameSet.insert(policy.GetName()); \
|
||||
} \
|
||||
bool HasPolicy(const std::string& name) const \
|
||||
{ \
|
||||
return mPolicyNameSet.find(name) != mPolicyNameSet.end(); \
|
||||
} \
|
||||
void CopyToNativeQos(NativeQosType& qos) const; \
|
||||
private: \
|
||||
void InitPolicyDefault(); \
|
||||
private: \
|
||||
std::set<std::string> mPolicyNameSet; \
|
||||
};
|
||||
|
||||
/*
|
||||
* DdsParticipantQos
|
||||
*/
|
||||
UT_DECL_DDS_QOS(DdsParticipantQos, DdsNative<::dds::domain::qos::DomainParticipantQos>)
|
||||
|
||||
/*
|
||||
* DdsTopicQos
|
||||
*/
|
||||
UT_DECL_DDS_QOS(DdsTopicQos, DdsNative<::dds::topic::qos::TopicQos>)
|
||||
|
||||
/*
|
||||
* DdsPublisherQos
|
||||
*/
|
||||
UT_DECL_DDS_QOS(DdsPublisherQos, DdsNative<::dds::pub::qos::PublisherQos>)
|
||||
|
||||
/*
|
||||
* DdsSubscriberQos
|
||||
*/
|
||||
UT_DECL_DDS_QOS(DdsSubscriberQos, DdsNative<::dds::sub::qos::SubscriberQos>)
|
||||
|
||||
/*
|
||||
* DdsWriterQos
|
||||
*/
|
||||
UT_DECL_DDS_QOS(DdsWriterQos, DdsNative<::dds::pub::qos::DataWriterQos>)
|
||||
|
||||
/*
|
||||
* DdsReaderQos
|
||||
*/
|
||||
UT_DECL_DDS_QOS(DdsReaderQos, DdsNative<::dds::sub::qos::DataReaderQos>)
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
#endif//__UT_DDS_QOS_HPP__
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
#ifndef __UT_DDS_QOS_PARAMETER_HPP__
|
||||
#define __UT_DDS_QOS_PARAMETER_HPP__
|
||||
|
||||
#include <unitree/common/dds/dds_qos_policy_parameter.hpp>
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
class DdsQosParameter
|
||||
{
|
||||
public:
|
||||
DdsQosParameter();
|
||||
~DdsQosParameter();
|
||||
|
||||
void Init(const JsonMap& data);
|
||||
|
||||
#define __GET_POLICY(POLICY) \
|
||||
const DdsQos##POLICY##PolicyParameter& Get##POLICY() const \
|
||||
{ \
|
||||
return m##POLICY; \
|
||||
}
|
||||
|
||||
__GET_POLICY(Deadline)
|
||||
__GET_POLICY(DestinationOrder)
|
||||
__GET_POLICY(Durability)
|
||||
__GET_POLICY(DurabilityService)
|
||||
__GET_POLICY(EntityFactory)
|
||||
__GET_POLICY(GroupData)
|
||||
__GET_POLICY(History)
|
||||
__GET_POLICY(LatencyBudget)
|
||||
__GET_POLICY(Lifespan)
|
||||
__GET_POLICY(Liveliness)
|
||||
__GET_POLICY(Ownership)
|
||||
__GET_POLICY(OwnershipStrength)
|
||||
__GET_POLICY(Partition)
|
||||
__GET_POLICY(Presentation)
|
||||
__GET_POLICY(ReaderDataLifecycle)
|
||||
__GET_POLICY(Reliability)
|
||||
__GET_POLICY(ResourceLimits)
|
||||
__GET_POLICY(TimeBasedFilter)
|
||||
__GET_POLICY(TopicData)
|
||||
__GET_POLICY(TransportPriority)
|
||||
__GET_POLICY(WriterDataLifecycle)
|
||||
__GET_POLICY(UserData)
|
||||
|
||||
#undef __GET_POLICY
|
||||
|
||||
bool Default() const;
|
||||
|
||||
private:
|
||||
bool mDefault;
|
||||
|
||||
DdsQosDeadlinePolicyParameter mDeadline;
|
||||
DdsQosDestinationOrderPolicyParameter mDestinationOrder;
|
||||
DdsQosDurabilityPolicyParameter mDurability;
|
||||
DdsQosDurabilityServicePolicyParameter mDurabilityService;
|
||||
DdsQosEntityFactoryPolicyParameter mEntityFactory;
|
||||
DdsQosGroupDataPolicyParameter mGroupData;
|
||||
DdsQosHistoryPolicyParameter mHistory;
|
||||
DdsQosLatencyBudgetPolicyParameter mLatencyBudget;
|
||||
DdsQosLifespanPolicyParameter mLifespan;
|
||||
DdsQosLivelinessPolicyParameter mLiveliness;
|
||||
DdsQosOwnershipPolicyParameter mOwnership;
|
||||
DdsQosOwnershipStrengthPolicyParameter mOwnershipStrength;
|
||||
DdsQosPartitionPolicyParameter mPartition;
|
||||
DdsQosPresentationPolicyParameter mPresentation;
|
||||
DdsQosReaderDataLifecyclePolicyParameter mReaderDataLifecycle;
|
||||
DdsQosReliabilityPolicyParameter mReliability;
|
||||
DdsQosResourceLimitsPolicyParameter mResourceLimits;
|
||||
DdsQosTimeBasedFilterPolicyParameter mTimeBasedFilter;
|
||||
DdsQosTopicDataPolicyParameter mTopicData;
|
||||
DdsQosTransportPriorityPolicyParameter mTransportPriority;
|
||||
DdsQosWriterDataLifecyclePolicyParameter mWriterDataLifecycle;
|
||||
DdsQosUserDataPolicyParameter mUserData;
|
||||
};
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
#endif//__UT_DDS_QOS_PARAMETER_HPP__
|
||||
|
|
@ -1,195 +0,0 @@
|
|||
#ifndef __UT_DDS_QOS_POLICY_HPP__
|
||||
#define __UT_DDS_QOS_POLICY_HPP__
|
||||
|
||||
#include <dds/dds.hpp>
|
||||
#include <unitree/common/dds/dds_native.hpp>
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
class DdsQosPolicyName
|
||||
{
|
||||
public:
|
||||
explicit DdsQosPolicyName(const std::string& name) :
|
||||
mName(name)
|
||||
{}
|
||||
|
||||
virtual ~DdsQosPolicyName()
|
||||
{}
|
||||
|
||||
const std::string& GetName() const
|
||||
{
|
||||
return mName;
|
||||
}
|
||||
|
||||
protected:
|
||||
std::string mName;
|
||||
};
|
||||
|
||||
class DdsDuration : public DdsNative<::dds::core::Duration>
|
||||
{
|
||||
public:
|
||||
explicit DdsDuration(int64_t nanoSecond);
|
||||
~DdsDuration();
|
||||
};
|
||||
|
||||
class DdsQosDeadlinePolicy : public DdsNative<::dds::core::policy::Deadline>, public DdsQosPolicyName
|
||||
{
|
||||
public:
|
||||
explicit DdsQosDeadlinePolicy(int64_t period);
|
||||
~DdsQosDeadlinePolicy();
|
||||
};
|
||||
|
||||
class DdsQosDestinationOrderPolicy : public DdsNative<::dds::core::policy::DestinationOrder>, public DdsQosPolicyName
|
||||
{
|
||||
public:
|
||||
explicit DdsQosDestinationOrderPolicy(int32_t kind);
|
||||
~DdsQosDestinationOrderPolicy();
|
||||
};
|
||||
|
||||
class DdsQosDurabilityPolicy : public DdsNative<::dds::core::policy::Durability>, public DdsQosPolicyName
|
||||
{
|
||||
public:
|
||||
explicit DdsQosDurabilityPolicy(int32_t kind);
|
||||
~DdsQosDurabilityPolicy();
|
||||
};
|
||||
|
||||
class DdsQosDurabilityServicePolicy : public DdsNative<::dds::core::policy::DurabilityService>, public DdsQosPolicyName
|
||||
{
|
||||
public:
|
||||
explicit DdsQosDurabilityServicePolicy(int64_t cleanupDelay, int32_t historyKind, int32_t historyDepth,
|
||||
int32_t maxSamples, int32_t maxInstances, int32_t maxSamplesPerInstance);
|
||||
~DdsQosDurabilityServicePolicy();
|
||||
};
|
||||
|
||||
class DdsQosEntityFactoryPolicy : public DdsNative<::dds::core::policy::EntityFactory>, public DdsQosPolicyName
|
||||
{
|
||||
public:
|
||||
explicit DdsQosEntityFactoryPolicy(bool autoEnable);
|
||||
~DdsQosEntityFactoryPolicy();
|
||||
};
|
||||
|
||||
class DdsQosGroupDataPolicy : public DdsNative<::dds::core::policy::GroupData>, public DdsQosPolicyName
|
||||
{
|
||||
public:
|
||||
explicit DdsQosGroupDataPolicy(const std::vector<uint8_t>& value);
|
||||
~DdsQosGroupDataPolicy();
|
||||
};
|
||||
|
||||
class DdsQosHistoryPolicy : public DdsNative<::dds::core::policy::History>, public DdsQosPolicyName
|
||||
{
|
||||
public:
|
||||
explicit DdsQosHistoryPolicy(int32_t kind, int32_t depth);
|
||||
~DdsQosHistoryPolicy();
|
||||
};
|
||||
|
||||
class DdsQosLatencyBudgetPolicy : public DdsNative<::dds::core::policy::LatencyBudget>, public DdsQosPolicyName
|
||||
{
|
||||
public:
|
||||
explicit DdsQosLatencyBudgetPolicy(int64_t duration);
|
||||
~DdsQosLatencyBudgetPolicy();
|
||||
};
|
||||
|
||||
class DdsQosLifespanPolicy : public DdsNative<::dds::core::policy::Lifespan>, public DdsQosPolicyName
|
||||
{
|
||||
public:
|
||||
explicit DdsQosLifespanPolicy(int64_t duration);
|
||||
~DdsQosLifespanPolicy();
|
||||
};
|
||||
|
||||
class DdsQosLivelinessPolicy : public DdsNative<::dds::core::policy::Liveliness>, public DdsQosPolicyName
|
||||
{
|
||||
public:
|
||||
explicit DdsQosLivelinessPolicy(int32_t kind, int64_t leaseDuration);
|
||||
~DdsQosLivelinessPolicy();
|
||||
};
|
||||
|
||||
class DdsQosOwnershipPolicy : public DdsNative<::dds::core::policy::Ownership>, public DdsQosPolicyName
|
||||
{
|
||||
public:
|
||||
explicit DdsQosOwnershipPolicy(int32_t kind);
|
||||
~DdsQosOwnershipPolicy();
|
||||
};
|
||||
|
||||
class DdsQosOwnershipStrengthPolicy : public DdsNative<::dds::core::policy::OwnershipStrength>, public DdsQosPolicyName
|
||||
{
|
||||
public:
|
||||
explicit DdsQosOwnershipStrengthPolicy(int32_t strength);
|
||||
~DdsQosOwnershipStrengthPolicy();
|
||||
};
|
||||
|
||||
class DdsQosPartitionPolicy : public DdsNative<::dds::core::policy::Partition>, public DdsQosPolicyName
|
||||
{
|
||||
public:
|
||||
explicit DdsQosPartitionPolicy(const std::string& name);
|
||||
~DdsQosPartitionPolicy();
|
||||
};
|
||||
|
||||
class DdsQosPresentationPolicy : public DdsNative<::dds::core::policy::Presentation>, public DdsQosPolicyName
|
||||
{
|
||||
public:
|
||||
explicit DdsQosPresentationPolicy(int32_t accessScopeKind, bool coherentAccess, bool orderedAccess);
|
||||
~DdsQosPresentationPolicy();
|
||||
};
|
||||
|
||||
class DdsQosReaderDataLifecyclePolicy : public DdsNative<::dds::core::policy::ReaderDataLifecycle>, public DdsQosPolicyName
|
||||
{
|
||||
public:
|
||||
explicit DdsQosReaderDataLifecyclePolicy(int64_t autopurgeNowriterSamplesDelay, int64_t autopurgeDisposedSamplesDelay);
|
||||
~DdsQosReaderDataLifecyclePolicy();
|
||||
};
|
||||
|
||||
class DdsQosReliabilityPolicy : public DdsNative<::dds::core::policy::Reliability>, public DdsQosPolicyName
|
||||
{
|
||||
public:
|
||||
explicit DdsQosReliabilityPolicy(int32_t kind, int64_t maxBlockingTime);
|
||||
~DdsQosReliabilityPolicy();
|
||||
};
|
||||
|
||||
class DdsQosResourceLimitsPolicy : public DdsNative<::dds::core::policy::ResourceLimits>, public DdsQosPolicyName
|
||||
{
|
||||
public:
|
||||
explicit DdsQosResourceLimitsPolicy(int32_t maxSamples, int32_t maxInstances, int32_t maxSamplesPerInstance);
|
||||
~DdsQosResourceLimitsPolicy();
|
||||
};
|
||||
|
||||
class DdsQosTimeBasedFilterPolicy : public DdsNative<::dds::core::policy::TimeBasedFilter>, public DdsQosPolicyName
|
||||
{
|
||||
public:
|
||||
explicit DdsQosTimeBasedFilterPolicy(int64_t minSep);
|
||||
~DdsQosTimeBasedFilterPolicy();
|
||||
};
|
||||
|
||||
class DdsQosTopicDataPolicy : public DdsNative<::dds::core::policy::TopicData>, public DdsQosPolicyName
|
||||
{
|
||||
public:
|
||||
explicit DdsQosTopicDataPolicy(const std::vector<uint8_t>& value);
|
||||
~DdsQosTopicDataPolicy();
|
||||
};
|
||||
|
||||
class DdsQosTransportPriorityPolicy : public DdsNative<::dds::core::policy::TransportPriority>, public DdsQosPolicyName
|
||||
{
|
||||
public:
|
||||
explicit DdsQosTransportPriorityPolicy(int32_t value);
|
||||
~DdsQosTransportPriorityPolicy();
|
||||
};
|
||||
|
||||
class DdsQosWriterDataLifecyclePolicy : public DdsNative<::dds::core::policy::WriterDataLifecycle>, public DdsQosPolicyName
|
||||
{
|
||||
public:
|
||||
explicit DdsQosWriterDataLifecyclePolicy(bool autodisposeUnregisteredInstances);
|
||||
~DdsQosWriterDataLifecyclePolicy();
|
||||
};
|
||||
|
||||
class DdsQosUserDataPolicy : public DdsNative<::dds::core::policy::UserData>, public DdsQosPolicyName
|
||||
{
|
||||
public:
|
||||
explicit DdsQosUserDataPolicy(const std::vector<uint8_t>& value);
|
||||
~DdsQosUserDataPolicy();
|
||||
};
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
#endif//__UT_DDS_QOS_POLICY_HPP__
|
||||
|
|
@ -1,607 +0,0 @@
|
|||
#ifndef __UT_DDS_QOS_POLICY_PARAMETER_HPP__
|
||||
#define __UT_DDS_QOS_POLICY_PARAMETER_HPP__
|
||||
|
||||
#include <unitree/common/json/json.hpp>
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
/*
|
||||
*----------------------------
|
||||
* Supported Qos policy:
|
||||
* 1. Deadline
|
||||
* IDL:
|
||||
* struct DeadlineQosPolicy {
|
||||
* Duration_t period;
|
||||
* };
|
||||
*
|
||||
* 2. DestinationOrder
|
||||
* IDL:
|
||||
* enum DestinationOrderQosPolicyKind {
|
||||
* BY_RECEPTION_TIMESTAMP_DESTINATIONORDER_QOS,
|
||||
* BY_SOURCE_TIMESTAMP_DESTINATIONORDER_QOS
|
||||
* };
|
||||
* struct DestinationOrderQosPolicy {
|
||||
* DestinationOrderQosPolicyKind kind;
|
||||
* };
|
||||
*
|
||||
* 3. Durability
|
||||
* IDL:
|
||||
* enum DurabilityQosPolicyKind {
|
||||
* VOLATILE_DURABILITY_QOS, // Least Durability
|
||||
* TRANSIENT_LOCAL_DURABILITY_QOS,
|
||||
* TRANSIENT_DURABILITY_QOS,
|
||||
* PERSISTENT_DURABILITY_QOS // Greatest Durability
|
||||
* };
|
||||
* struct DurabilityQosPolicy {
|
||||
* DurabilityQosPolicyKind kind;
|
||||
* };
|
||||
*
|
||||
* 4. DurabilityService
|
||||
* IDL:
|
||||
* struct DurabilityServiceQosPolicy {
|
||||
* Duration_t service_cleanup_delay;
|
||||
* HistoryQosPolicyKind history_kind;
|
||||
* long history_depth;
|
||||
* long max_samples;
|
||||
* long max_instances;
|
||||
* long max_samples_per_instance;
|
||||
* };
|
||||
*
|
||||
* 5. EntityFactory
|
||||
* IDL:
|
||||
* struct EntityFactoryQosPolicy {
|
||||
* boolean autoenable_created_entities;
|
||||
* };
|
||||
*
|
||||
* 6. GroupData
|
||||
* IDL:
|
||||
* struct GroupDataQosPolicy {
|
||||
* sequence<octet> value;
|
||||
* };
|
||||
*
|
||||
* 7. History
|
||||
* IDL:
|
||||
* enum HistoryQosPolicyKind {
|
||||
* KEEP_LAST_HISTORY_QOS,
|
||||
* KEEP_ALL_HISTORY_QOS
|
||||
* };
|
||||
* struct HistoryQosPolicy {
|
||||
* HistoryQosPolicyKind kind;
|
||||
* long depth;
|
||||
* };
|
||||
* 8. LatencyBudget
|
||||
* IDL:
|
||||
* struct LatencyBudgetQosPolicy {
|
||||
* Duration_t duration;
|
||||
* };
|
||||
*
|
||||
* 9. Lifespan
|
||||
* IDL:
|
||||
* struct LifespanQosPolicy {
|
||||
* Duration_t duration;
|
||||
* }
|
||||
*
|
||||
* 10.Liveliness
|
||||
* IDL:
|
||||
* enum LivelinessQosPolicyKind {
|
||||
* AUTOMATIC_LIVELINESS_QOS,
|
||||
* MANUAL_BY_PARTICIPANT_LIVELINESS_QOS,
|
||||
* MANUAL_BY_TOPIC_LIVELINESS_QOS
|
||||
* };
|
||||
* struct LivelinessQosPolicy {
|
||||
* LivelinessQosPolicyKind kind;
|
||||
* Duration_t lease_duration;
|
||||
* };
|
||||
*
|
||||
* 11.Ownership
|
||||
* IDL:
|
||||
* enum OwnershipQosPolicyKind {
|
||||
* SHARED_OWNERSHIP_QOS,
|
||||
* EXCLUSIVE_OWNERSHIP_QOS
|
||||
* };
|
||||
* struct OwnershipQosPolicy {
|
||||
* OwnershipQosPolicyKind kind;
|
||||
* };
|
||||
*
|
||||
* 12.OwnershipStrength
|
||||
* IDL:
|
||||
* struct OwnershipStrengthQosPolicy {
|
||||
* long value;
|
||||
* };
|
||||
*
|
||||
* 13.Partition
|
||||
* IDL:
|
||||
* struct PartitionQosPolicy {
|
||||
* StringSeq name;
|
||||
* };
|
||||
*
|
||||
* 14.Presentation
|
||||
* IDL:
|
||||
* enum PresentationQosPolicyAccessScopeKind {
|
||||
* INSTANCE_PRESENTATION_QOS,
|
||||
* TOPIC_PRESENTATION_QOS,
|
||||
* GROUP_PRESENTATION_QOS
|
||||
* };
|
||||
* struct PresentationQosPolicy {
|
||||
* PresentationQosPolicyAccessScopeKind access_scope;
|
||||
* boolean coherent_access;
|
||||
* boolean ordered_access;
|
||||
* };
|
||||
*
|
||||
* 15.ReaderDataLifecycle
|
||||
* IDL:
|
||||
* struct ReaderDataLifecycleQosPolicy {
|
||||
* Duration_t autopurge_nowriter_samples_delay;
|
||||
* Duration_t autopurge_disposed_samples_delay;
|
||||
* };
|
||||
*
|
||||
* 16.Reliability
|
||||
* IDL:
|
||||
* enum ReliabilityQosPolicyKind {
|
||||
* BEST_EFFORT_RELIABILITY_QOS,
|
||||
* RELIABLE_RELIABILITY_QOS
|
||||
* };
|
||||
* struct ReliabilityQosPolicy {
|
||||
* ReliabilityQosPolicyKind kind;
|
||||
* Duration_t max_blocking_time;
|
||||
* };
|
||||
* 17.ResourceLimits
|
||||
* IDL:
|
||||
* struct ResourceLimitsQosPolicy {
|
||||
* long max_samples;
|
||||
* long max_instances;
|
||||
* long max_samples_per_instance;
|
||||
* };
|
||||
*
|
||||
* 18.TimeBasedFilter
|
||||
* IDL:
|
||||
* struct TimeBasedFilterQosPolicy {
|
||||
* Duration_t minimum_separation;
|
||||
* };
|
||||
*
|
||||
* 19.TopicData
|
||||
* IDL:
|
||||
* struct TopicDataQosPolicy {
|
||||
* sequence<octet> value;
|
||||
* };
|
||||
*
|
||||
* 20.TransportPriority
|
||||
* IDL:
|
||||
* struct TransportPriorityQosPolicy {
|
||||
* long value;
|
||||
* };
|
||||
*
|
||||
* 21.UserData
|
||||
* IDL:
|
||||
* struct UserDataQosPolicy {
|
||||
* sequence<octet> value;
|
||||
* };
|
||||
*
|
||||
* 22.WriterDataLifecycle
|
||||
* IDL:
|
||||
* struct WriterDataLifecycleQosPolicy {
|
||||
* boolean autodispose_unregistered_instances;
|
||||
* };
|
||||
*
|
||||
*-------------------------------------------------------------------------------------------------
|
||||
* QoS policis corresponding to entity:
|
||||
* DomainParticipant: [2] { EntityFactory, UserData }
|
||||
* Topic: [13] { Deadline, DestinationOrder, Durability, DurabilityService, History,
|
||||
* LatencyBudget, Lifespan, Liveliness, Ownership, Reliability,
|
||||
* ResourceLimits, TopicData, TransportPriority }
|
||||
* Publisher: [4] { EntityFactory, GroupData, Partition, Presentation }
|
||||
* Subscriber: [4] { EntityFactory, GroupData, Partition, Presentation }
|
||||
* DataWriter: [14] { Deadline, DestinationOrder, Durability, History, LatencyBudget,
|
||||
* Lifespan, Liveliness, Ownership, OwnershipStrength, Reliability,
|
||||
* ResourceLimits, TransportPriority, UserData, WriterDataLifecycle }
|
||||
* DataReader: [12] { Deadline, DestinationOrder, Durability, History, LatencyBudget
|
||||
* Liveliness, Ownership, Reliability, ReaderDataLifecycle, ResourceLimits,
|
||||
* TimeBasedFilter, UserData }
|
||||
*-------------------------------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
/*
|
||||
* Qos policy name
|
||||
*/
|
||||
#define UT_DDS_QOS_POLICY_NAME_DEADLINE "Deadline"
|
||||
#define UT_DDS_QOS_POLICY_NAME_DESTINATION_ORDER "DestinationOrder"
|
||||
#define UT_DDS_QOS_POLICY_NAME_DURABILITY "Durability"
|
||||
#define UT_DDS_QOS_POLICY_NAME_DURABILITY_SERVICE "DurabilityService"
|
||||
#define UT_DDS_QOS_POLICY_NAME_ENTITY_FACTORY "EntityFactory"
|
||||
#define UT_DDS_QOS_POLICY_NAME_GROUP_DATA "GroupData"
|
||||
#define UT_DDS_QOS_POLICY_NAME_HISTORY "History"
|
||||
#define UT_DDS_QOS_POLICY_NAME_LATENCY_BUDGET "LatencyBudget"
|
||||
#define UT_DDS_QOS_POLICY_NAME_LIFESPAN "Lifespan"
|
||||
#define UT_DDS_QOS_POLICY_NAME_LIVELINESS "Liveliness"
|
||||
#define UT_DDS_QOS_POLICY_NAME_OWNERSHIP "Ownership"
|
||||
#define UT_DDS_QOS_POLICY_NAME_OWNERSHIP_STRENGTH "OwnershipStrength"
|
||||
#define UT_DDS_QOS_POLICY_NAME_PARTITION "Partition"
|
||||
#define UT_DDS_QOS_POLICY_NAME_PRESENTATION "Presentation"
|
||||
#define UT_DDS_QOS_POLICY_NAME_READER_DATA_LIFECYCLE "ReaderDataLifecycle"
|
||||
#define UT_DDS_QOS_POLICY_NAME_RELIABILITY "Reliability"
|
||||
#define UT_DDS_QOS_POLICY_NAME_RESOURCE_LIMITS "ResourceLimits"
|
||||
#define UT_DDS_QOS_POLICY_NAME_TIME_BASED_FILTER "TimeBasedFilter"
|
||||
#define UT_DDS_QOS_POLICY_NAME_TOPIC_DATA "TopicData"
|
||||
#define UT_DDS_QOS_POLICY_NAME_TRANSPORT_PRIORITY "TransportPriority"
|
||||
#define UT_DDS_QOS_POLICY_NAME_WRITER_DATA_LIFECYCLE "WriterDataLifecycle"
|
||||
#define UT_DDS_QOS_POLICY_NAME_USER_DATA "UserData"
|
||||
|
||||
/*
|
||||
* Qos Policy Member Name
|
||||
*/
|
||||
#define UT_DDS_QOS_POLICY_MEMBER_NAME_KIND "kind"
|
||||
#define UT_DDS_QOS_POLICY_MEMBER_NAME_PEROID "peroid"
|
||||
#define UT_DDS_QOS_POLICY_MEMBER_NAME_CLEANUP_DELAY "service_cleanup_delay"
|
||||
#define UT_DDS_QOS_POLICY_MEMBER_NAME_HISTORY_KIND "history_kind"
|
||||
#define UT_DDS_QOS_POLICY_MEMBER_NAME_HISTORY_DEPTH "history_depth"
|
||||
#define UT_DDS_QOS_POLICY_MEMBER_NAME_MAX_SAMPLES "max_samples"
|
||||
#define UT_DDS_QOS_POLICY_MEMBER_NAME_MAX_INSTANCES "max_instances"
|
||||
#define UT_DDS_QOS_POLICY_MEMBER_NAME_MAX_SAMPLES_PER_INSTANCE "max_samples_per_instance"
|
||||
#define UT_DDS_QOS_POLICY_MEMBER_NAME_AUTO_ENABLE "autoenable_created_entities"
|
||||
#define UT_DDS_QOS_POLICY_MEMBER_NAME_VALUE "value"
|
||||
#define UT_DDS_QOS_POLICY_MEMBER_NAME_DEPTH "depth"
|
||||
#define UT_DDS_QOS_POLICY_MEMBER_NAME_DURATION "duration"
|
||||
#define UT_DDS_QOS_POLICY_MEMBER_NAME_LEASE_DURATION "lease_duration"
|
||||
#define UT_DDS_QOS_POLICY_MEMBER_NAME_NAME "name"
|
||||
#define UT_DDS_QOS_POLICY_MEMBER_NAME_ACCESS_SCOPE "access_scope"
|
||||
#define UT_DDS_QOS_POLICY_MEMBER_NAME_COHERENT_ACCESS "coherent_access"
|
||||
#define UT_DDS_QOS_POLICY_MEMBER_NAME_ORDERED_ACCESS "ordered_access"
|
||||
#define UT_DDS_QOS_POLICY_MEMBER_NAME_MAX_BLOCKING_TIME "max_blocking_time"
|
||||
#define UT_DDS_QOS_POLICY_MEMBER_NAME_AUTODISPOSE_UNREGISETED_INSTANCES "autodispose_unregistered_instances"
|
||||
#define UT_DDS_QOS_POLICY_MEMBER_NAME_AUTOPURGE_NOWRITER_SAMPLES_DELAY "autopurge_nowriter_samples_delay"
|
||||
#define UT_DDS_QOS_POLICY_MEMBER_NAME_AUTOPURGE_DISPOSED_SAMPLES_DELAY "autopurge_disposed_samples_delay"
|
||||
#define UT_DDS_QOS_POLICY_MEMBER_NAME_MIN_SEP "minimum_separation"
|
||||
|
||||
class DdsQosPolicyParameter
|
||||
{
|
||||
public:
|
||||
DdsQosPolicyParameter();
|
||||
virtual ~DdsQosPolicyParameter();
|
||||
|
||||
virtual void Init(const JsonMap& data) = 0;
|
||||
|
||||
void Update();
|
||||
bool Default() const;
|
||||
|
||||
protected:
|
||||
bool mDefault;
|
||||
};
|
||||
|
||||
class DdsQosDeadlinePolicyParameter : public DdsQosPolicyParameter
|
||||
{
|
||||
public:
|
||||
DdsQosDeadlinePolicyParameter();
|
||||
~DdsQosDeadlinePolicyParameter();
|
||||
|
||||
void Init(const JsonMap& data);
|
||||
int64_t GetPeriod() const;
|
||||
|
||||
private:
|
||||
int64_t mPeriod;
|
||||
};
|
||||
|
||||
class DdsQosDestinationOrderPolicyParameter : public DdsQosPolicyParameter
|
||||
{
|
||||
public:
|
||||
DdsQosDestinationOrderPolicyParameter();
|
||||
~DdsQosDestinationOrderPolicyParameter();
|
||||
|
||||
void Init(const JsonMap& data);
|
||||
|
||||
int32_t GetKind() const;
|
||||
|
||||
private:
|
||||
int32_t mKind;
|
||||
};
|
||||
|
||||
class DdsQosDurabilityPolicyParameter : public DdsQosPolicyParameter
|
||||
{
|
||||
public:
|
||||
DdsQosDurabilityPolicyParameter();
|
||||
~DdsQosDurabilityPolicyParameter();
|
||||
|
||||
void Init(const JsonMap& data);
|
||||
|
||||
int32_t GetKind() const;
|
||||
|
||||
private:
|
||||
int32_t mKind;
|
||||
};
|
||||
|
||||
class DdsQosDurabilityServicePolicyParameter : public DdsQosPolicyParameter
|
||||
{
|
||||
public:
|
||||
DdsQosDurabilityServicePolicyParameter();
|
||||
~DdsQosDurabilityServicePolicyParameter();
|
||||
|
||||
void Init(const JsonMap& data);
|
||||
|
||||
int64_t GetCleanupDelay() const;
|
||||
int32_t GetHistoryKind() const;
|
||||
int32_t GetHistoryDepth() const;
|
||||
int32_t GetMaxSamples() const;
|
||||
int32_t GetMaxInstances() const;
|
||||
int32_t GetMaxSamplesPerInstance() const;
|
||||
|
||||
private:
|
||||
int64_t mCleanupDelay;
|
||||
int32_t mHistoryKind;
|
||||
int32_t mHistoryDepth;
|
||||
int32_t mMaxSamples;
|
||||
int32_t mMaxInstances;
|
||||
int32_t mMaxSamplesPerInstance;
|
||||
};
|
||||
|
||||
class DdsQosEntityFactoryPolicyParameter : public DdsQosPolicyParameter
|
||||
{
|
||||
public:
|
||||
DdsQosEntityFactoryPolicyParameter();
|
||||
~DdsQosEntityFactoryPolicyParameter();
|
||||
|
||||
void Init(const JsonMap& data);
|
||||
|
||||
int64_t GetAutoEnable() const;
|
||||
|
||||
private:
|
||||
bool mAutoEnable;
|
||||
};
|
||||
|
||||
class DdsQosGroupDataPolicyParameter: public DdsQosPolicyParameter
|
||||
{
|
||||
public:
|
||||
DdsQosGroupDataPolicyParameter();
|
||||
~DdsQosGroupDataPolicyParameter();
|
||||
|
||||
void Init(const JsonMap& data);
|
||||
const std::vector<uint8_t>& GetValue() const;
|
||||
|
||||
private:
|
||||
std::vector<uint8_t> mValue;
|
||||
};
|
||||
|
||||
class DdsQosHistoryPolicyParameter : public DdsQosPolicyParameter
|
||||
{
|
||||
public:
|
||||
DdsQosHistoryPolicyParameter();
|
||||
~DdsQosHistoryPolicyParameter();
|
||||
|
||||
void Init(const JsonMap& data);
|
||||
|
||||
int32_t GetKind() const;
|
||||
int32_t GetDepth() const;
|
||||
|
||||
private:
|
||||
int32_t mKind;
|
||||
int32_t mDepth;
|
||||
};
|
||||
|
||||
class DdsQosLatencyBudgetPolicyParameter : public DdsQosPolicyParameter
|
||||
{
|
||||
public:
|
||||
DdsQosLatencyBudgetPolicyParameter();
|
||||
~DdsQosLatencyBudgetPolicyParameter();
|
||||
|
||||
void Init(const JsonMap& data);
|
||||
|
||||
int64_t GetDuration() const;
|
||||
|
||||
private:
|
||||
int64_t mDuration;
|
||||
};
|
||||
|
||||
class DdsQosLifespanPolicyParameter : public DdsQosPolicyParameter
|
||||
{
|
||||
public:
|
||||
DdsQosLifespanPolicyParameter();
|
||||
~DdsQosLifespanPolicyParameter();
|
||||
|
||||
void Init(const JsonMap& data);
|
||||
|
||||
int64_t GetDuration() const;
|
||||
|
||||
private:
|
||||
int64_t mDuration;
|
||||
};
|
||||
|
||||
class DdsQosLivelinessPolicyParameter : public DdsQosPolicyParameter
|
||||
{
|
||||
public:
|
||||
DdsQosLivelinessPolicyParameter();
|
||||
~DdsQosLivelinessPolicyParameter();
|
||||
|
||||
void Init(const JsonMap& data);
|
||||
|
||||
int32_t GetKind() const;
|
||||
int64_t GetLeaseDuration() const;
|
||||
|
||||
private:
|
||||
int32_t mKind;
|
||||
int64_t mLeaseDuration;
|
||||
};
|
||||
|
||||
class DdsQosOwnershipPolicyParameter : public DdsQosPolicyParameter
|
||||
{
|
||||
public:
|
||||
DdsQosOwnershipPolicyParameter();
|
||||
~DdsQosOwnershipPolicyParameter();
|
||||
|
||||
void Init(const JsonMap& data);
|
||||
|
||||
int32_t GetKind() const;
|
||||
|
||||
private:
|
||||
int32_t mKind;
|
||||
};
|
||||
|
||||
class DdsQosOwnershipStrengthPolicyParameter : public DdsQosPolicyParameter
|
||||
{
|
||||
public:
|
||||
DdsQosOwnershipStrengthPolicyParameter();
|
||||
~DdsQosOwnershipStrengthPolicyParameter();
|
||||
|
||||
void Init(const JsonMap& data);
|
||||
|
||||
int32_t GetValue() const;
|
||||
|
||||
private:
|
||||
int32_t mValue;
|
||||
};
|
||||
|
||||
class DdsQosPartitionPolicyParameter : public DdsQosPolicyParameter
|
||||
{
|
||||
public:
|
||||
DdsQosPartitionPolicyParameter();
|
||||
~DdsQosPartitionPolicyParameter();
|
||||
|
||||
void Init(const JsonMap& data);
|
||||
|
||||
const std::string& GetName() const;
|
||||
|
||||
private:
|
||||
std::string mName;
|
||||
};
|
||||
|
||||
class DdsQosPresentationPolicyParameter : public DdsQosPolicyParameter
|
||||
{
|
||||
public:
|
||||
DdsQosPresentationPolicyParameter();
|
||||
~DdsQosPresentationPolicyParameter();
|
||||
|
||||
void Init(const JsonMap& data);
|
||||
|
||||
int32_t GetAccessScopeKind() const;
|
||||
bool GetCoherentAccess() const;
|
||||
bool GetOrderedAccess() const;
|
||||
|
||||
private:
|
||||
int32_t mAccessScopeKind;
|
||||
bool mCoherentAccess;
|
||||
bool mOrderedAccess;
|
||||
};
|
||||
|
||||
class DdsQosReaderDataLifecyclePolicyParameter : public DdsQosPolicyParameter
|
||||
{
|
||||
public:
|
||||
DdsQosReaderDataLifecyclePolicyParameter();
|
||||
~DdsQosReaderDataLifecyclePolicyParameter();
|
||||
|
||||
void Init(const JsonMap& data);
|
||||
|
||||
int64_t GetAutopurgeNowriterSamplesDelay() const;
|
||||
int64_t GetAutopurgeDisposedSamplesDelay() const;
|
||||
|
||||
private:
|
||||
int64_t mAutopurgeNowriterSamplesDelay;
|
||||
int64_t mAutopurgeDisposedSamplesDelay;
|
||||
};
|
||||
|
||||
class DdsQosReliabilityPolicyParameter : public DdsQosPolicyParameter
|
||||
{
|
||||
public:
|
||||
DdsQosReliabilityPolicyParameter();
|
||||
~DdsQosReliabilityPolicyParameter();
|
||||
|
||||
void Init(const JsonMap& data);
|
||||
|
||||
int32_t GetKind() const;
|
||||
int64_t GetMaxBlockingTime() const;
|
||||
|
||||
private:
|
||||
int32_t mKind;
|
||||
int64_t mMaxBlockingTime;
|
||||
};
|
||||
|
||||
class DdsQosResourceLimitsPolicyParameter : public DdsQosPolicyParameter
|
||||
{
|
||||
public:
|
||||
DdsQosResourceLimitsPolicyParameter();
|
||||
~DdsQosResourceLimitsPolicyParameter();
|
||||
|
||||
void Init(const JsonMap& data);
|
||||
|
||||
int32_t GetMaxSamples() const;
|
||||
int32_t GetMaxInstances() const;
|
||||
int32_t GetMaxSamplesPerInstance() const;
|
||||
|
||||
private:
|
||||
int32_t mMaxSamples;
|
||||
int32_t mMaxInstances;
|
||||
int32_t mMaxSamplesPerInstance;
|
||||
};
|
||||
|
||||
class DdsQosTimeBasedFilterPolicyParameter : public DdsQosPolicyParameter
|
||||
{
|
||||
public:
|
||||
DdsQosTimeBasedFilterPolicyParameter();
|
||||
~DdsQosTimeBasedFilterPolicyParameter();
|
||||
|
||||
void Init(const JsonMap& data);
|
||||
|
||||
int64_t GetMinSep() const;
|
||||
|
||||
private:
|
||||
int64_t mMinSep;
|
||||
};
|
||||
|
||||
class DdsQosTopicDataPolicyParameter: public DdsQosPolicyParameter
|
||||
{
|
||||
public:
|
||||
DdsQosTopicDataPolicyParameter();
|
||||
~DdsQosTopicDataPolicyParameter();
|
||||
|
||||
void Init(const JsonMap& data);
|
||||
|
||||
const std::vector<uint8_t>& GetValue() const;
|
||||
|
||||
private:
|
||||
std::vector<uint8_t> mValue;
|
||||
};
|
||||
|
||||
class DdsQosTransportPriorityPolicyParameter : public DdsQosPolicyParameter
|
||||
{
|
||||
public:
|
||||
DdsQosTransportPriorityPolicyParameter();
|
||||
~DdsQosTransportPriorityPolicyParameter();
|
||||
|
||||
void Init(const JsonMap& data);
|
||||
|
||||
int32_t GetValue() const;
|
||||
|
||||
private:
|
||||
int32_t mValue;
|
||||
};
|
||||
|
||||
class DdsQosWriterDataLifecyclePolicyParameter : public DdsQosPolicyParameter
|
||||
{
|
||||
public:
|
||||
DdsQosWriterDataLifecyclePolicyParameter();
|
||||
~DdsQosWriterDataLifecyclePolicyParameter();
|
||||
|
||||
void Init(const JsonMap& data);
|
||||
|
||||
bool GetAutodisposeUnregisteredInstances() const;
|
||||
|
||||
private:
|
||||
bool mAutodisposeUnregisteredInstances;
|
||||
};
|
||||
|
||||
class DdsQosUserDataPolicyParameter: public DdsQosPolicyParameter
|
||||
{
|
||||
public:
|
||||
DdsQosUserDataPolicyParameter();
|
||||
~DdsQosUserDataPolicyParameter();
|
||||
|
||||
void Init(const JsonMap& data);
|
||||
|
||||
const std::vector<uint8_t>& GetValue() const;
|
||||
|
||||
private:
|
||||
std::vector<uint8_t> mValue;
|
||||
};
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
#endif//__UT_DDS_QOS_POLICY_PARAMETER_HPP__
|
||||
|
|
@ -1,26 +0,0 @@
|
|||
#ifndef __UT_DDS_QOS_REALIZE_HPP__
|
||||
#define __UT_DDS_QOS_REALIZE_HPP__
|
||||
|
||||
#include <unitree/common/dds/dds_qos_parameter.hpp>
|
||||
#include <unitree/common/dds/dds_qos.hpp>
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
void Realize(const DdsQosParameter& parameter, DdsParticipantQos& qos);
|
||||
|
||||
void Realize(const DdsQosParameter& parameter, DdsTopicQos& qos);
|
||||
|
||||
void Realize(const DdsQosParameter& parameter, DdsPublisherQos& qos);
|
||||
|
||||
void Realize(const DdsQosParameter& parameter, DdsSubscriberQos& qos);
|
||||
|
||||
void Realize(const DdsQosParameter& parameter, DdsWriterQos& qos);
|
||||
|
||||
void Realize(const DdsQosParameter& parameter, DdsReaderQos& qos);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
#endif//__UT_DDS_QOS_REALIZE_HPP__
|
||||
|
|
@ -1,96 +0,0 @@
|
|||
#ifndef __UT_DDS_TOPIC_CHANNEL_HPP__
|
||||
#define __UT_DDS_TOPIC_CHANNEL_HPP__
|
||||
|
||||
#include <unitree/common/dds/dds_entity.hpp>
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
/*
|
||||
* @brief: DdsTopicChannelAbstract
|
||||
*/
|
||||
class DdsTopicChannelAbstract
|
||||
{
|
||||
public:
|
||||
virtual bool Write(const void* message, int64_t waitMicrosec) = 0;
|
||||
virtual int64_t GetLastDataAvailableTime() const = 0;
|
||||
};
|
||||
|
||||
using DdsTopicChannelAbstractPtr = std::shared_ptr<DdsTopicChannelAbstract>;
|
||||
|
||||
#define UT_DDS_WAIT_MATCHED_TIME_MICRO_SEC 100000
|
||||
|
||||
/*
|
||||
* @brief: DdsTopicChannel
|
||||
*/
|
||||
template<typename MSG>
|
||||
class DdsTopicChannel : public DdsTopicChannelAbstract
|
||||
{
|
||||
public:
|
||||
explicit DdsTopicChannel()
|
||||
{}
|
||||
|
||||
~DdsTopicChannel()
|
||||
{}
|
||||
|
||||
void SetTopic(const DdsParticipantPtr& participant, const std::string& name, const DdsTopicQos& qos)
|
||||
{
|
||||
mTopic = DdsTopicPtr<MSG>(new DdsTopic<MSG>(participant, name, qos));
|
||||
}
|
||||
|
||||
void SetWriter(const DdsPublisherPtr& publisher, const DdsWriterQos& qos)
|
||||
{
|
||||
mWriter = DdsWriterPtr<MSG>(new DdsWriter<MSG>(publisher, mTopic, qos));
|
||||
MicroSleep(UT_DDS_WAIT_MATCHED_TIME_MICRO_SEC);
|
||||
}
|
||||
|
||||
void SetReader(const DdsSubscriberPtr& subscriber, const DdsReaderQos& qos, const DdsReaderCallback& cb, int32_t queuelen)
|
||||
{
|
||||
mReader = DdsReaderPtr<MSG>(new DdsReader<MSG>(subscriber, mTopic, qos));
|
||||
mReader->SetListener(cb, queuelen);
|
||||
}
|
||||
|
||||
DdsWriterPtr<MSG> GetWriter() const
|
||||
{
|
||||
return mWriter;
|
||||
}
|
||||
|
||||
DdsReaderPtr<MSG> GetReader() const
|
||||
{
|
||||
return mReader;
|
||||
}
|
||||
|
||||
bool Write(const void* message, int64_t waitMicrosec)
|
||||
{
|
||||
return Write(*(const MSG*)message, waitMicrosec);
|
||||
}
|
||||
|
||||
bool Write(const MSG& message, int64_t waitMicrosec)
|
||||
{
|
||||
return mWriter->Write(message, waitMicrosec);
|
||||
}
|
||||
|
||||
int64_t GetLastDataAvailableTime() const
|
||||
{
|
||||
if (mReader)
|
||||
{
|
||||
return mReader->GetLastDataAvailableTime();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
private:
|
||||
DdsTopicPtr<MSG> mTopic;
|
||||
DdsWriterPtr<MSG> mWriter;
|
||||
DdsReaderPtr<MSG> mReader;
|
||||
};
|
||||
|
||||
template<typename MSG>
|
||||
using DdsTopicChannelPtr = std::shared_ptr<DdsTopicChannel<MSG>>;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
#endif//__UT_DDS_TOPIC_CHANNEL_HPP__
|
||||
|
|
@ -1,18 +0,0 @@
|
|||
#ifndef __UT_DDS_TRAINTS_HPP__
|
||||
#define __UT_DDS_TRAINTS_HPP__
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
#include <org/eclipse/cyclonedds/topic/TopicTraits.hpp>
|
||||
|
||||
#define DdsGetTypeName(TYPE) \
|
||||
org::eclipse::cyclonedds::topic::TopicTraits<TYPE>::getTypeName()
|
||||
|
||||
#define DdsIsKeyless(TYPE) \
|
||||
org::eclipse::cyclonedds::topic::TopicTraits<TYPE>::isKeyless()
|
||||
|
||||
}
|
||||
}
|
||||
#endif//__UT_DDS_TRAINTS_HPP__
|
||||
|
|
@ -1,91 +0,0 @@
|
|||
#ifndef __UT_DECL_HPP__
|
||||
#define __UT_DECL_HPP__
|
||||
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdint.h>
|
||||
#include <errno.h>
|
||||
#include <exception>
|
||||
#include <execinfo.h>
|
||||
#include <sched.h>
|
||||
#include <signal.h>
|
||||
#include <string.h>
|
||||
#include <strings.h>
|
||||
#include <arpa/inet.h>
|
||||
#include <ifaddrs.h>
|
||||
#include <sys/mman.h>
|
||||
#include <sys/stat.h>
|
||||
#include <sys/statvfs.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/time.h>
|
||||
#include <sys/socket.h>
|
||||
#include <sys/un.h>
|
||||
#include <sys/sysinfo.h>
|
||||
#include <sys/syscall.h>
|
||||
#include <sys/resource.h>
|
||||
#include <sys/timerfd.h>
|
||||
#include <netinet/in.h>
|
||||
#include <netdb.h>
|
||||
#include <time.h>
|
||||
#include <poll.h>
|
||||
#include <pthread.h>
|
||||
#include <pwd.h>
|
||||
#include <limits.h>
|
||||
#include <fcntl.h>
|
||||
#include <dirent.h>
|
||||
#include <utime.h>
|
||||
#include <atomic>
|
||||
#include <string>
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
#include <typeinfo>
|
||||
#include <list>
|
||||
#include <vector>
|
||||
#include <set>
|
||||
#include <map>
|
||||
#include <unordered_map>
|
||||
#include <functional>
|
||||
#include <iomanip>
|
||||
#include <memory>
|
||||
#include <regex>
|
||||
|
||||
#ifdef __GLIBC__
|
||||
#define UT_UNLIKELY(x) __builtin_expect(!!(x), 0)
|
||||
#define UT_LIKELY(x) __builtin_expect(!!(x), 1)
|
||||
#else
|
||||
#define UT_UNLIKELY(x) (x)
|
||||
#define UT_LIKELY(x) (x)
|
||||
#endif//__GLIBC__
|
||||
|
||||
#define __UT_CAT(x, y) x##y
|
||||
#define UT_CAT(x, y) __UT_CAT(x, y)
|
||||
|
||||
#define __UT_STR(x) #x
|
||||
#define UT_STR(x) __UT_STR(x)
|
||||
|
||||
#define UT_QUEUE_MAX_LEN INT_MAX
|
||||
#define UT_PATH_MAX_LEN 1024
|
||||
#define UT_THREAD_NAME_MAX_LEN 16
|
||||
|
||||
#define UT_DECL_ERR(name, code, desc) \
|
||||
const int32_t name = code; const std::string name##_DESC = desc;
|
||||
|
||||
#define UT_DESC_ERR(name) name##_DESC
|
||||
|
||||
#if defined(__GLIBC__) and defined(__aarch64__)
|
||||
#define gettid() syscall(__NR_gettid)
|
||||
#endif
|
||||
|
||||
#define UT_SAFE_DEL(x) \
|
||||
if ((x) != NULL) { delete (x); (x) = NULL; }
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
static const std::string UT_EMPTY_STR = "";
|
||||
}
|
||||
}
|
||||
|
||||
#endif//__UT_DECL_HPP__
|
||||
|
|
@ -1,25 +0,0 @@
|
|||
#ifndef __UT_ERROR_HPP__
|
||||
#define __UT_ERROR_HPP__
|
||||
|
||||
#include <unitree/common/decl.hpp>
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
//Declare error
|
||||
UT_DECL_ERR(UT_OK, 0, "Success")
|
||||
UT_DECL_ERR(UT_ERR_COMMON, 1001, "common error")
|
||||
UT_DECL_ERR(UT_ERR_BADCAST, 1002, "Bad cast error")
|
||||
UT_DECL_ERR(UT_ERR_FUTURE, 1003, "Future error")
|
||||
UT_DECL_ERR(UT_ERR_FUTURE_FAULT,1004, "Future fault error")
|
||||
UT_DECL_ERR(UT_ERR_JSON, 1005, "Json data error")
|
||||
UT_DECL_ERR(UT_ERR_SYSTEM, 1006, "System error")
|
||||
UT_DECL_ERR(UT_ERR_FILE, 1007, "File operation error")
|
||||
UT_DECL_ERR(UT_ERR_SOCKET, 1008, "Socket operaton error")
|
||||
UT_DECL_ERR(UT_ERR_IO, 1009, "IO operaton error")
|
||||
UT_DECL_ERR(UT_ERR_LOCK, 1010, "Lock operation error")
|
||||
UT_DECL_ERR(UT_ERR_NETWORK, 1011, "Network error")
|
||||
UT_DECL_ERR(UT_ERR_TIMEOUT, 1012, "Timeout error")
|
||||
UT_DECL_ERR(UT_ERR_UNKNOWN, -1, "Unknown error")
|
||||
}
|
||||
|
||||
#endif//__UT_ERROR_HPP__
|
||||
|
|
@ -1,203 +0,0 @@
|
|||
#ifndef __UT_EXCEPTION_HPP__
|
||||
#define __UT_EXCEPTION_HPP__
|
||||
|
||||
#include <unitree/common/error.hpp>
|
||||
|
||||
#define UT_MAX_TRACE_ADDR_NUMBER 64
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
class Exception : public std::exception
|
||||
{
|
||||
public:
|
||||
Exception() throw()
|
||||
: Exception(UT_ERR_UNKNOWN, UT_DESC_ERR(UT_ERR_UNKNOWN))
|
||||
{}
|
||||
|
||||
Exception(int32_t code, const std::string& message) throw()
|
||||
: mCode(code), mMessage(message), mLine(0)
|
||||
{}
|
||||
|
||||
virtual ~Exception()
|
||||
{}
|
||||
|
||||
int32_t GetCode() const
|
||||
{
|
||||
return mCode;
|
||||
}
|
||||
|
||||
const std::string& GetMessage() const
|
||||
{
|
||||
return mMessage;
|
||||
}
|
||||
|
||||
virtual const char* what() const noexcept
|
||||
{
|
||||
return mMessage.c_str();
|
||||
}
|
||||
|
||||
virtual std::string GetClassName() const
|
||||
{
|
||||
return "Exception";
|
||||
}
|
||||
|
||||
void Init(const char* file, const char* func, int32_t line)
|
||||
{
|
||||
mFile = file;
|
||||
mFunc = func;
|
||||
mLine = line;
|
||||
}
|
||||
|
||||
std::string ToString() const
|
||||
{
|
||||
std::ostringstream os;
|
||||
AddDetail(os);
|
||||
|
||||
return os.str();
|
||||
}
|
||||
|
||||
std::string StackTrace() const
|
||||
{
|
||||
std::ostringstream os;
|
||||
AddDetail(os);
|
||||
AddBackTrace(os);
|
||||
|
||||
return os.str();
|
||||
}
|
||||
|
||||
protected:
|
||||
void AddDetail(std::ostringstream& os) const
|
||||
{
|
||||
os << "Catch " << GetClassName() << " code:" << mCode
|
||||
<< ", message:" << mMessage << std::endl;
|
||||
|
||||
if (!mFile.empty() && !mFunc.empty() && mLine > 0)
|
||||
{
|
||||
os << " at __FILE__:" << mFile << ", __LINE__:"
|
||||
<< mLine << ", __FUNCTION__:" << mFunc << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void AddBackTrace(std::ostringstream& os) const
|
||||
{
|
||||
os << "Stack:" << std::endl;
|
||||
|
||||
void* addr[UT_MAX_TRACE_ADDR_NUMBER];
|
||||
int32_t number = backtrace(addr, UT_MAX_TRACE_ADDR_NUMBER);
|
||||
|
||||
char **info = backtrace_symbols(addr, number);
|
||||
if(info == NULL)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
for(int32_t i=0; i<number; i++)
|
||||
{
|
||||
os << info[i] << std::endl;
|
||||
}
|
||||
|
||||
free(info);
|
||||
}
|
||||
|
||||
protected:
|
||||
int32_t mCode;
|
||||
std::string mMessage;
|
||||
|
||||
std::string mFile;
|
||||
std::string mFunc;
|
||||
int32_t mLine;
|
||||
|
||||
std::string mWhat;
|
||||
};
|
||||
|
||||
#define UT_THROW_0(ExceptionType) \
|
||||
do \
|
||||
{ \
|
||||
ExceptionType __temp_except_r38e2d; \
|
||||
__temp_except_r38e2d.Init(__FILE__,__PRETTY_FUNCTION__,__LINE__); \
|
||||
throw(__temp_except_r38e2d); \
|
||||
} while(0);
|
||||
|
||||
#define UT_THROW(ExceptionType, args...) \
|
||||
do \
|
||||
{ \
|
||||
ExceptionType __temp_except_r38e2d(args); \
|
||||
__temp_except_r38e2d.Init(__FILE__,__PRETTY_FUNCTION__,__LINE__); \
|
||||
throw(__temp_except_r38e2d); \
|
||||
} while(0);
|
||||
|
||||
#define UT_THROW_IF(condition, ExceptionType, args...) \
|
||||
if (condition) \
|
||||
{ \
|
||||
UT_THROW(ExceptionType, args); \
|
||||
}
|
||||
|
||||
#define UT_EXCEPTION_TRY \
|
||||
try \
|
||||
{
|
||||
|
||||
#define __UT_EXCEPTION_CATCH(except, l, t) \
|
||||
catch (const except& e) \
|
||||
{ \
|
||||
if (l) \
|
||||
{ \
|
||||
LOG_ERROR(l, e.what()); \
|
||||
} \
|
||||
if (t) \
|
||||
{ \
|
||||
throw e; \
|
||||
} \
|
||||
}
|
||||
|
||||
#define UT_EXCEPTION_CATCH(l, t) \
|
||||
} \
|
||||
__UT_EXCEPTION_CATCH(unitree::common::Exception, l, t) \
|
||||
__UT_EXCEPTION_CATCH(std::exception, l, t)
|
||||
|
||||
#define UT_DECL_EXCEPTION(ExceptionType, code, desc) \
|
||||
class ExceptionType : public Exception \
|
||||
{ \
|
||||
public: \
|
||||
ExceptionType() throw() \
|
||||
: Exception(code, desc) \
|
||||
{} \
|
||||
\
|
||||
ExceptionType(const std::string& message) throw() \
|
||||
: Exception(code, message) \
|
||||
{} \
|
||||
\
|
||||
std::string GetClassName() const \
|
||||
{ \
|
||||
return #ExceptionType; \
|
||||
} \
|
||||
};
|
||||
|
||||
UT_DECL_EXCEPTION(CommonException, UT_ERR_COMMON, UT_DESC_ERR(UT_ERR_COMMON))
|
||||
|
||||
UT_DECL_EXCEPTION(SystemException, UT_ERR_SYSTEM, UT_DESC_ERR(UT_ERR_SYSTEM))
|
||||
|
||||
UT_DECL_EXCEPTION(NetworkException, UT_ERR_NETWORK, UT_DESC_ERR(UT_ERR_NETWORK))
|
||||
|
||||
UT_DECL_EXCEPTION(FileException, UT_ERR_FILE, UT_DESC_ERR(UT_ERR_FILE))
|
||||
|
||||
UT_DECL_EXCEPTION(SocketException, UT_ERR_SOCKET, UT_DESC_ERR(UT_ERR_SOCKET))
|
||||
|
||||
UT_DECL_EXCEPTION(IOException, UT_ERR_IO, UT_DESC_ERR(UT_ERR_IO))
|
||||
|
||||
UT_DECL_EXCEPTION(LockException, UT_ERR_LOCK, UT_DESC_ERR(UT_ERR_LOCK))
|
||||
|
||||
UT_DECL_EXCEPTION(TimeoutException, UT_ERR_TIMEOUT, UT_DESC_ERR(UT_ERR_TIMEOUT))
|
||||
|
||||
UT_DECL_EXCEPTION(BadCastException, UT_ERR_BADCAST, UT_DESC_ERR(UT_ERR_BADCAST))
|
||||
|
||||
UT_DECL_EXCEPTION(JsonException, UT_ERR_JSON, UT_DESC_ERR(UT_ERR_JSON))
|
||||
|
||||
UT_DECL_EXCEPTION(FutureException, UT_ERR_FUTURE, UT_DESC_ERR(UT_ERR_FUTURE))
|
||||
|
||||
UT_DECL_EXCEPTION(FutureFaultException, UT_ERR_FUTURE_FAULT, UT_DESC_ERR(UT_ERR_FUTURE_FAULT))
|
||||
|
||||
}
|
||||
}
|
||||
#endif//__UT_EXCEPTION_HPP__
|
||||
|
|
@ -1,68 +0,0 @@
|
|||
#ifndef __UT_DIRECTORY_HPP__
|
||||
#define __UT_DIRECTORY_HPP__
|
||||
|
||||
#include <unitree/common/filesystem/filesystem.hpp>
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
typedef std::pair<int32_t,std::string> DIRENT_INFO;
|
||||
typedef std::pair<std::string,std::string> SYMLNK_INFO;
|
||||
|
||||
class Directory
|
||||
{
|
||||
public:
|
||||
Directory();
|
||||
Directory(const std::string& dirName);
|
||||
|
||||
~Directory();
|
||||
|
||||
void Open();
|
||||
void Open(const std::string& dirName);
|
||||
|
||||
bool IsOpen();
|
||||
|
||||
void ListFile(std::vector<std::string>& fileNameList, const std::string& regExpress);
|
||||
void ListFile(std::vector<std::string>& fileNameList, bool recurse = true, bool absolute = true);
|
||||
|
||||
void ListDir(std::vector<std::string>& dirNameList, bool recurse = true, bool absolute = true);
|
||||
|
||||
void List(std::list<std::string>& fileNameList, std::list<std::string>& dirNameList,
|
||||
std::list<SYMLNK_INFO>& symlinkList);
|
||||
|
||||
void Cleanup();
|
||||
bool CopyTo(const std::string& dirName, bool deeply = false);
|
||||
|
||||
void Close();
|
||||
|
||||
private:
|
||||
void CheckOpen();
|
||||
void OpenInner();
|
||||
|
||||
private:
|
||||
DIR *mDIR;
|
||||
std::string mDirName;
|
||||
};
|
||||
|
||||
typedef std::shared_ptr<Directory> DirectoryPtr;
|
||||
|
||||
bool ExistDirectory(const std::string& dirName);
|
||||
|
||||
void CreateDirectory(const std::string& dirName, bool recurse = true,
|
||||
uint32_t mode = UT_OPEN_MODE_RWX);
|
||||
|
||||
void ListDirectory(const std::string& dirName, std::vector<std::string>& fileNameList,
|
||||
const std::string& regExpress);
|
||||
|
||||
void ListDirectory(const std::string& dirName, std::vector<std::string>& fileNameList, bool recurse = true, bool absolute = true);
|
||||
|
||||
void ListChildDirectory(const std::string& dirName, std::vector<std::string>& dirNameList, bool recurse = true, bool absolute = true);
|
||||
|
||||
void RemoveDirectory(const std::string& dirName, bool recurse = true);
|
||||
|
||||
void CopyDirectory(const std::string& dirName, const std::string& destDirName, bool deeply);
|
||||
|
||||
}
|
||||
}
|
||||
#endif//__UT_DIRECTORY_HPP__
|
||||
|
|
@ -1,127 +0,0 @@
|
|||
#ifndef __UT_FILE_HPP__
|
||||
#define __UT_FILE_HPP__
|
||||
|
||||
#include <unitree/common/filesystem/filesystem.hpp>
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
class File
|
||||
{
|
||||
public:
|
||||
File();
|
||||
File(const std::string& fileName);
|
||||
File(const std::string& fileName, int32_t flag);
|
||||
File(const std::string& fileName, int32_t flag, uint32_t mode);
|
||||
|
||||
virtual ~File();
|
||||
|
||||
int32_t GetFd() const;
|
||||
bool IsOpen() const;
|
||||
|
||||
void Open();
|
||||
void Open(int32_t flag, uint32_t mode = UT_OPEN_MODE_NONE);
|
||||
void Open(const std::string& fileName, int32_t flag = UT_OPEN_FLAG_R,
|
||||
uint32_t mode = UT_OPEN_MODE_NONE);
|
||||
|
||||
void Append(const char* s, int64_t len);
|
||||
int64_t Write(const char* s, int64_t len);
|
||||
int64_t Write(const std::string& s);
|
||||
|
||||
int64_t Read(char* s, int64_t len);
|
||||
int64_t Read(std::string& s, int64_t len);
|
||||
int64_t ReadAll(std::string& s);
|
||||
|
||||
void SeekBegin();
|
||||
void SeekEnd();
|
||||
void Seek(int64_t offset, int64_t whence);
|
||||
|
||||
void Sync();
|
||||
void Truncate(int64_t len);
|
||||
|
||||
void Close();
|
||||
|
||||
int64_t Size() const;
|
||||
|
||||
private:
|
||||
void CheckOpen();
|
||||
void OpenInner();
|
||||
|
||||
private:
|
||||
std::string mFileName;
|
||||
int32_t mFd;
|
||||
int32_t mFlag;
|
||||
uint32_t mMode;
|
||||
};
|
||||
|
||||
typedef std::shared_ptr<File> FilePtr;
|
||||
|
||||
class MMReadFile
|
||||
{
|
||||
public:
|
||||
MMReadFile();
|
||||
MMReadFile(const std::string& fileName);
|
||||
|
||||
virtual ~MMReadFile();
|
||||
|
||||
int32_t GetFd() const;
|
||||
bool IsOpen() const;
|
||||
|
||||
void Open();
|
||||
void Open(const std::string& fileName);
|
||||
|
||||
int64_t Size() const;
|
||||
int64_t Read(char* s, int64_t len);
|
||||
int64_t Read(std::string& s, int64_t len);
|
||||
int64_t ReadAll(std::string& s);
|
||||
|
||||
void Seek(int64_t offset);
|
||||
|
||||
void Close();
|
||||
|
||||
public:
|
||||
void* MMRead(int64_t len, int64_t& canreadlen);
|
||||
void MMClose(void* ptr, int64_t len);
|
||||
|
||||
private:
|
||||
void Init();
|
||||
|
||||
private:
|
||||
std::string mFileName;
|
||||
int32_t mFd;
|
||||
int64_t mOffset;
|
||||
int64_t mSize;
|
||||
};
|
||||
|
||||
typedef std::shared_ptr<MMReadFile> MMReadFilePtr;
|
||||
|
||||
//Functions
|
||||
bool ExistFile(const std::string& fileName);
|
||||
|
||||
std::string LoadFile(const std::string& fileName);
|
||||
std::string LoadFileEx(const std::string& fileName);
|
||||
|
||||
void SaveFile(const std::string& fileName, const char* s, int64_t len,
|
||||
uint32_t mode = UT_OPEN_MODE_RW);
|
||||
|
||||
void SaveFile(const std::string& fileName, const std::string& s,
|
||||
uint32_t mode = UT_OPEN_MODE_RW);
|
||||
|
||||
void AppendFile(const std::string& fileName, const char* s, int64_t len,
|
||||
uint32_t mode = UT_OPEN_MODE_RW);
|
||||
|
||||
void AppendFile(const std::string& fileName, const std::string& s,
|
||||
uint32_t mode = UT_OPEN_MODE_RW);
|
||||
|
||||
int64_t GetFileSize(const std::string& fileName);
|
||||
|
||||
void RemoveFile(const std::string& fileName);
|
||||
|
||||
int64_t MMLoadFile(const std::string& fileName, std::string& s);
|
||||
|
||||
bool CopyFile(const std::string& fileName, const std::string& destFileName, bool deeply = false);
|
||||
}
|
||||
}
|
||||
|
||||
#endif//__UT_FILE_HPP__
|
||||
|
|
@ -1,284 +0,0 @@
|
|||
#ifndef __UT_FILE_SYSTEM_HPP__
|
||||
#define __UT_FILE_SYSTEM_HPP__
|
||||
|
||||
#include <unitree/common/decl.hpp>
|
||||
|
||||
#define UT_FILE_READ_SIZE 65536 //64K
|
||||
#define UT_FILE_READ_BIGGER_SIZE 262144 //256K
|
||||
|
||||
#define UT_FD_INVALID -1
|
||||
#define UT_FD_STDIN STDIN_FILENO
|
||||
#define UT_FD_STDOUT STDOUT_FILENO
|
||||
#define UT_FD_STDERR STDERR_FILENO
|
||||
|
||||
#define UT_PATH_DELIM_CHAR '/'
|
||||
#define UT_PATH_DELIM_STR "/"
|
||||
#define UT_PATH_TRIM_DELIM_STR " \t\r\n"
|
||||
|
||||
#define NOT_FLAG(f, flag) \
|
||||
(((f) & (flag)) != (flag))
|
||||
|
||||
#define HAS_FLAG(f, flag) \
|
||||
(((f) & (flag)) == (flag))
|
||||
|
||||
#define SET_FLAG(f, flag) \
|
||||
(f) |= (flag)
|
||||
|
||||
#define IS_RDONLY_FLAG(f) \
|
||||
((f & O_ACCMODE) == O_RDONLY)
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
enum
|
||||
{
|
||||
UT_OPEN_FLAG_R = O_RDONLY,
|
||||
UT_OPEN_FLAG_W = O_WRONLY,
|
||||
UT_OPEN_FLAG_RW = O_RDWR,
|
||||
UT_OPEN_FLAG_T = O_TRUNC,
|
||||
UT_OPEN_FLAG_A = O_APPEND,
|
||||
UT_OPEN_FLAG_S = O_SYNC,
|
||||
UT_OPEN_FLAG_C = O_CREAT,
|
||||
UT_OPEN_FLAG_CW = O_CREAT | O_WRONLY,
|
||||
UT_OPEN_FLAG_CWT = O_CREAT | O_WRONLY | O_TRUNC,
|
||||
UT_OPEN_FLAG_CA = O_CREAT | O_RDWR | O_APPEND,
|
||||
UT_OPEN_FLAG_CS = O_CREAT | O_RDWR | O_SYNC,
|
||||
UT_OPEN_FLAG_CAS = O_CREAT | O_RDWR | O_APPEND | O_SYNC,
|
||||
UT_OPEN_FLAG_CTS = O_CREAT | O_RDWR | O_TRUNC | O_SYNC
|
||||
};
|
||||
|
||||
enum
|
||||
{
|
||||
UT_OPEN_MODE_NONE = 0,
|
||||
UT_OPEN_MODE_RW = S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP | S_IROTH | S_IWOTH,
|
||||
UT_OPEN_MODE_RWX = S_IRWXU | S_IRGRP | S_IXGRP | S_IROTH | S_IXOTH
|
||||
};
|
||||
|
||||
enum
|
||||
{
|
||||
UT_OPEN_MASK_000 = 0,
|
||||
UT_OPEN_MASK_022 = 0022
|
||||
};
|
||||
|
||||
typedef struct dirent DIRENT;
|
||||
|
||||
/*
|
||||
* FileSystemHelper
|
||||
*/
|
||||
class FileSystemHelper
|
||||
{
|
||||
public:
|
||||
static FileSystemHelper* Instance()
|
||||
{
|
||||
static FileSystemHelper inst;
|
||||
return &inst;
|
||||
}
|
||||
|
||||
/*
|
||||
* File Functions
|
||||
*/
|
||||
int32_t Open(const std::string& fileName, int32_t flag, uint32_t mode = 0);
|
||||
|
||||
int64_t Write(int32_t fd, const char* s, int64_t len);
|
||||
int64_t Write(int32_t fd, const std::string& s);
|
||||
|
||||
int64_t Read(int32_t fd, char* s, int64_t len);
|
||||
int64_t Read(int32_t fd, std::string& s, int64_t len);
|
||||
int64_t ReadAll(int32_t fd, std::string& s);
|
||||
|
||||
int64_t Seek(int32_t fd, int64_t offset, int64_t whence);
|
||||
|
||||
bool Stat(int32_t fd, struct stat& statbuf);
|
||||
void Truncate(int32_t fd, int64_t len);
|
||||
void Sync(int32_t fd);
|
||||
void Close(int32_t fd);
|
||||
|
||||
/*
|
||||
* Directory Functions
|
||||
*/
|
||||
DIR* Opendir(const std::string& dirName);
|
||||
DIRENT* Readdir(DIR* dir);
|
||||
void Closedir(DIR* dir);
|
||||
|
||||
void Makedir(const std::string& dirName, uint32_t mode = UT_OPEN_MODE_RWX,
|
||||
bool ignoreExist = true);
|
||||
void MakedirRecurse(const std::string& dirName, uint32_t mode = UT_OPEN_MODE_RWX);
|
||||
|
||||
/*
|
||||
* stat Function
|
||||
*/
|
||||
bool Stat(const std::string& name, struct stat& statbuf);
|
||||
bool StatL(const std::string& name, struct stat& statbuf);
|
||||
bool IsFile(const struct stat& statbuf);
|
||||
bool IsDirectory(const struct stat& statbuf);
|
||||
bool IsSymlink(const struct stat& statbuf);
|
||||
bool ExistFile(const std::string& name);
|
||||
bool ExistDirectory(const std::string& name);
|
||||
bool Exist(const std::string& name);
|
||||
|
||||
int64_t GetFileSize(const std::string& fileName);
|
||||
|
||||
/*
|
||||
* Symlink Function
|
||||
*/
|
||||
std::string GetSymlink(const std::string& symlinkName);
|
||||
bool Symlink(const std::string& targetFileName, const std::string& symlinkName);
|
||||
|
||||
/*
|
||||
* Remove and Rename Function
|
||||
*/
|
||||
void RemoveFile(const std::string& fileName, bool ignoreNoExist = true);
|
||||
void RemoveDirectory(const std::string& dirName, bool ignoreNoExist = true);
|
||||
|
||||
void Remove(const std::string& name, bool ignoreNoExist = true);
|
||||
void Rename(const std::string& oldName, const std::string& newName);
|
||||
|
||||
/*
|
||||
* Path proccess Function
|
||||
*/
|
||||
std::string& NormalizePath(std::string& s, bool withEndDelim = true);
|
||||
std::string GetFatherDirectory(const std::string& s, bool withEndDelim = true);
|
||||
std::string GetFileName(const std::string& s);
|
||||
std::string GetLastName(const std::string& s);
|
||||
|
||||
/*
|
||||
* Realpath
|
||||
*/
|
||||
std::string GetRealName(const std::string& name);
|
||||
|
||||
bool IsSame(const std::string& name1, const std::string& name2);
|
||||
|
||||
/*
|
||||
* MMap Function
|
||||
*/
|
||||
void* MmapRead(int32_t fd, int64_t offset, int64_t len);
|
||||
void Munmap(void *addr, int64_t len);
|
||||
|
||||
/*
|
||||
* Mode and Owner
|
||||
*/
|
||||
bool Chmod(const std::string& name, mode_t mode);
|
||||
bool Chown(const std::string& name, uid_t uid, gid_t gid);
|
||||
|
||||
bool ChmodL(const std::string& name, mode_t mode);
|
||||
bool ChownL(const std::string& name, uid_t uid, gid_t gid);
|
||||
|
||||
bool UTime(const std::string& name, struct utimbuf& times);
|
||||
bool UTime(const std::string& name, struct timeval times[2]);
|
||||
|
||||
bool Chattr(const std::string& name, const struct stat& statbuf);
|
||||
|
||||
/*
|
||||
* ParseModeString
|
||||
* @param s: like "0755", "0666", "766"
|
||||
* return mode
|
||||
*/
|
||||
uint32_t ParseModeString(const std::string& s);
|
||||
|
||||
private:
|
||||
FileSystemHelper();
|
||||
~FileSystemHelper();
|
||||
|
||||
private:
|
||||
int32_t mOldMask;
|
||||
};
|
||||
|
||||
/*
|
||||
* FDCloser
|
||||
*/
|
||||
class FDCloser
|
||||
{
|
||||
public:
|
||||
explicit FDCloser() :
|
||||
mFd(UT_FD_INVALID)
|
||||
{}
|
||||
|
||||
explicit FDCloser(int32_t fd) :
|
||||
mFd(fd)
|
||||
{}
|
||||
|
||||
~FDCloser()
|
||||
{
|
||||
Close();
|
||||
}
|
||||
|
||||
void SetFd(int32_t fd)
|
||||
{
|
||||
//close old fd
|
||||
Close();
|
||||
//set new fd
|
||||
mFd = fd;
|
||||
}
|
||||
|
||||
private:
|
||||
void Close()
|
||||
{
|
||||
if (mFd > 0)
|
||||
{
|
||||
FileSystemHelper::Instance()->Close(mFd);
|
||||
mFd = UT_FD_INVALID;
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
int32_t mFd;
|
||||
};
|
||||
|
||||
/*
|
||||
* DIRCloser
|
||||
*/
|
||||
class DIRCloser
|
||||
{
|
||||
public:
|
||||
explicit DIRCloser() :
|
||||
mDIR(NULL)
|
||||
{}
|
||||
|
||||
explicit DIRCloser(DIR* dir) :
|
||||
mDIR(dir)
|
||||
{}
|
||||
|
||||
~DIRCloser()
|
||||
{
|
||||
Close();
|
||||
}
|
||||
|
||||
void SetDir(DIR* dir)
|
||||
{
|
||||
//close old dir
|
||||
Close();
|
||||
//set new dir
|
||||
mDIR = dir;
|
||||
}
|
||||
|
||||
private:
|
||||
void Close()
|
||||
{
|
||||
if (mDIR != NULL)
|
||||
{
|
||||
FileSystemHelper::Instance()->Closedir(mDIR);
|
||||
mDIR = NULL;
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
DIR* mDIR;
|
||||
};
|
||||
|
||||
//Function
|
||||
void Remove(const std::string& name);
|
||||
void Rename(const std::string& oldName, const std::string& newName);
|
||||
|
||||
std::string NormalizePath(const std::string& s, bool withEndDelim = false);
|
||||
std::string GetFatherDirectory(const std::string& s, bool withEndDelim = false);
|
||||
std::string GetFileName(const std::string& s);
|
||||
std::string GetLastName(const std::string& s);
|
||||
|
||||
void Chattr(const std::string& name, const struct stat& statbuf);
|
||||
void Copyattr(const std::string& name, const std::string& templateName);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif//__UT_FILE_SYSTEM_HPP__
|
||||
|
|
@ -1,39 +0,0 @@
|
|||
#ifndef __UT_JSON_HPP__
|
||||
#define __UT_JSON_HPP__
|
||||
|
||||
#include <unitree/common/any.hpp>
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
typedef std::map<std::string,Any> JsonMap;
|
||||
typedef std::vector<Any> JsonArray;
|
||||
|
||||
Any FromJsonString(const std::string& s);
|
||||
|
||||
std::string ToJsonString(const Any& a, bool pretty = false);
|
||||
|
||||
static inline bool IsNull(const Any& a)
|
||||
{
|
||||
return a.Empty();
|
||||
}
|
||||
|
||||
static inline bool IsJsonArray(const Any& a)
|
||||
{
|
||||
return a.GetTypeInfo() == typeid(JsonArray);
|
||||
}
|
||||
|
||||
static inline bool IsJsonMap(const Any& a)
|
||||
{
|
||||
return a.GetTypeInfo() == typeid(JsonMap);
|
||||
}
|
||||
|
||||
static inline bool IsJsonObject(const Any& a)
|
||||
{
|
||||
return IsJsonMap(a);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
#endif//__UT_JSON_HPP__
|
||||
|
|
@ -1,193 +0,0 @@
|
|||
#ifndef __UT_JSON_CONFIG_HPP__
|
||||
#define __UT_JSON_CONFIG_HPP__
|
||||
|
||||
#include <unitree/common/json/json.hpp>
|
||||
|
||||
#define UT_JSON_CONF_KEY_PARAMETER "Parameter"
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
class JsonConfig
|
||||
{
|
||||
public:
|
||||
JsonConfig();
|
||||
virtual ~JsonConfig();
|
||||
|
||||
JsonConfig(const std::string& configFileName);
|
||||
|
||||
virtual void Parse(const std::string& configFileName);
|
||||
|
||||
//top-level field
|
||||
bool Has(const std::string& name) const;
|
||||
|
||||
//top-level field
|
||||
const Any& Get(const std::string& name) const;
|
||||
|
||||
//top-level field
|
||||
template<typename T>
|
||||
const T& Get(const std::string& name) const
|
||||
{
|
||||
return AnyCast<T>(Get(name));
|
||||
}
|
||||
|
||||
//top-level field
|
||||
template<typename T>
|
||||
T GetNumber(const std::string& name) const
|
||||
{
|
||||
return AnyNumberCast<T>(Get(name));
|
||||
}
|
||||
|
||||
//top-level field
|
||||
template<typename T>
|
||||
T Get(const std::string& name, const T& defValue) const
|
||||
{
|
||||
const Any& a = Get(name);
|
||||
|
||||
if (a.Empty())
|
||||
{
|
||||
return defValue;
|
||||
}
|
||||
else
|
||||
{
|
||||
return AnyCast<T>(a);
|
||||
}
|
||||
}
|
||||
|
||||
//top-level field
|
||||
template<typename T>
|
||||
T GetNumber(const std::string& name, const T& defValue) const
|
||||
{
|
||||
const Any& a = Get(name);
|
||||
|
||||
if (a.Empty())
|
||||
{
|
||||
return defValue;
|
||||
}
|
||||
else
|
||||
{
|
||||
return AnyNumberCast<T>(a);
|
||||
}
|
||||
}
|
||||
|
||||
//JsonMap field
|
||||
bool Has(const JsonMap& jsonMap, const std::string& name) const;
|
||||
|
||||
//JsonMap field
|
||||
const Any& Get(const JsonMap& jsonMap, const std::string& name) const;
|
||||
|
||||
//JsonMap field
|
||||
template<typename T>
|
||||
const T& Get(const JsonMap& jsonMap, const std::string& name) const
|
||||
{
|
||||
return AnyCast<T>(Get(jsonMap, name));
|
||||
}
|
||||
|
||||
//JsonMap field
|
||||
template<typename T>
|
||||
T GetNumber(const JsonMap& jsonMap, const std::string& name) const
|
||||
{
|
||||
return AnyNumberCast<T>(Get(jsonMap, name));
|
||||
}
|
||||
|
||||
//JsonMap field
|
||||
template<typename T>
|
||||
T Get(const JsonMap& jsonMap, const std::string& name, const T& defValue) const
|
||||
{
|
||||
const Any& a = Get(jsonMap, name);
|
||||
|
||||
if (a.Empty())
|
||||
{
|
||||
return defValue;
|
||||
}
|
||||
else
|
||||
{
|
||||
return AnyCast<T>(a);
|
||||
}
|
||||
}
|
||||
|
||||
//JsonMap field
|
||||
template<typename T>
|
||||
T GetNumber(const JsonMap& jsonMap, const std::string& name, const T& defValue) const
|
||||
{
|
||||
const Any& a = Get(jsonMap, name);
|
||||
|
||||
if (a.Empty())
|
||||
{
|
||||
return defValue;
|
||||
}
|
||||
else
|
||||
{
|
||||
return AnyNumberCast<T>(a);
|
||||
}
|
||||
}
|
||||
|
||||
//top-level field
|
||||
const Any& GetGlobalParameter(const std::string& name) const;
|
||||
|
||||
//top-level field: Parameter
|
||||
bool HasParameter(const std::string& name) const;
|
||||
|
||||
//top-level field: Parameter
|
||||
const JsonMap& GetParameter() const;
|
||||
|
||||
//get field/value from top-level field: Parameter
|
||||
const Any& GetParameter(const std::string& name) const;
|
||||
|
||||
//get field/value from top-level field: Parameter
|
||||
template<typename T>
|
||||
const T& GetParameter(const std::string& name) const
|
||||
{
|
||||
return AnyCast<T>(GetParameter(name));
|
||||
}
|
||||
|
||||
//get field/value from top-level field: Parameter
|
||||
template<typename T>
|
||||
T GetNumberParameter(const std::string& name) const
|
||||
{
|
||||
return AnyNumberCast<T>(GetParameter(name));
|
||||
}
|
||||
|
||||
//get field/value from top-level field: Parameter
|
||||
template<typename T>
|
||||
T GetParameter(const std::string& name, const T& defValue) const
|
||||
{
|
||||
const Any& a = GetParameter(name);
|
||||
|
||||
if (a.Empty())
|
||||
{
|
||||
return defValue;
|
||||
}
|
||||
else
|
||||
{
|
||||
return AnyCast<T>(a);
|
||||
}
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
T GetNumberParameter(const std::string& name, const T& defValue) const
|
||||
{
|
||||
const Any& a = GetParameter(name);
|
||||
|
||||
if (a.Empty())
|
||||
{
|
||||
return defValue;
|
||||
}
|
||||
else
|
||||
{
|
||||
return AnyNumberCast<T>(a);
|
||||
}
|
||||
}
|
||||
|
||||
protected:
|
||||
JsonMap mParameter;
|
||||
Any mContent;
|
||||
};
|
||||
|
||||
typedef std::shared_ptr<JsonConfig> JsonConfigPtr;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
#endif//__UT_JSON_CONFIG_HPP__
|
||||
|
|
@ -1,259 +0,0 @@
|
|||
#ifndef __UT_JSONIZE_HPP__
|
||||
#define __UT_JSONIZE_HPP__
|
||||
|
||||
#define JN_FROM_WEAK(m, name, value) \
|
||||
if (m.find(name) != m.end()) { value = m[name]; }
|
||||
|
||||
#define JN_FROM(m, name, value) \
|
||||
value = m[name]
|
||||
|
||||
#define JN_TO(m, name, value) \
|
||||
m[name] = value
|
||||
|
||||
#include <unitree/common/json/json.hpp>
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
template<typename T>
|
||||
void FromJson(const Any& a, T& t);
|
||||
|
||||
template<typename T>
|
||||
void ToJson(const T& value, Any& a);
|
||||
|
||||
template<typename T>
|
||||
void FromJsonString(const std::string& s, T& t)
|
||||
{
|
||||
Any a = FromJsonString(s);
|
||||
FromJson<T>(a, t);
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
std::string ToJsonString(const T& t, bool pretty = false)
|
||||
{
|
||||
Any a;
|
||||
ToJson<T>(t, a);
|
||||
return ToJsonString(a, pretty);
|
||||
}
|
||||
|
||||
class Jsonize
|
||||
{
|
||||
public:
|
||||
virtual void toJson(JsonMap& a) const = 0;
|
||||
virtual void fromJson(JsonMap& a) = 0;
|
||||
};
|
||||
|
||||
void FromAny(const Any& a, int32_t& value);
|
||||
|
||||
void FromAny(const Any& a, uint32_t& value);
|
||||
|
||||
void FromAny(const Any& a, int64_t& value);
|
||||
|
||||
void FromAny(const Any& a, uint64_t& value);
|
||||
|
||||
void FromAny(const Any& a, float& value);
|
||||
|
||||
void FromAny(const Any& a, double& value);
|
||||
|
||||
void FromAny(const Any& a, bool& value);
|
||||
|
||||
void FromAny(const Any& a, std::string& value);
|
||||
|
||||
void FromAny(const Any& a, JsonMap& value);
|
||||
|
||||
void FromAny(const Any& a, JsonArray& value);
|
||||
|
||||
void FromAny(const Any& a, Jsonize& value);
|
||||
|
||||
template<typename E>
|
||||
void FromAny(const Any& a, std::vector<E>& value)
|
||||
{
|
||||
if (a.Empty())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
const JsonArray& arr = AnyCast<JsonArray>(a);
|
||||
size_t i, count = arr.size();
|
||||
|
||||
for (i=0; i<count; i++)
|
||||
{
|
||||
E e;
|
||||
const Any& a_in = arr[i];
|
||||
FromJson<E>(a_in, e);
|
||||
value.push_back(std::move(e));
|
||||
}
|
||||
}
|
||||
|
||||
template<typename E>
|
||||
void FromAny(const Any& a, std::list<E>& value)
|
||||
{
|
||||
if (a.Empty())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
const JsonArray& arr = AnyCast<JsonArray>(a);
|
||||
size_t i, count = arr.size();
|
||||
|
||||
for (i=0; i<count; i++)
|
||||
{
|
||||
E e;
|
||||
const Any& a_in = arr[i];
|
||||
FromJson<E>(a_in, e);
|
||||
value.push_back(std::move(e));
|
||||
}
|
||||
}
|
||||
|
||||
template<typename E>
|
||||
void FromAny(const Any& a, std::set<E>& value)
|
||||
{
|
||||
if (a.Empty())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
const JsonArray& arr = AnyCast<JsonArray>(a);
|
||||
size_t i, count = arr.size();
|
||||
|
||||
for (i=0; i<count; i++)
|
||||
{
|
||||
E e;
|
||||
const Any& a_in = arr[i];
|
||||
FromJson<E>(a_in, e);
|
||||
value.insert(std::move(e));
|
||||
}
|
||||
}
|
||||
|
||||
template<typename E>
|
||||
void FromAny(const Any& a, std::map<std::string,E>& value)
|
||||
{
|
||||
if (a.Empty())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
const JsonMap& m = AnyCast<JsonMap>(a);
|
||||
JsonMap::const_iterator iter;
|
||||
|
||||
for (iter = m.begin(); iter != m.end(); ++iter)
|
||||
{
|
||||
E e;
|
||||
const std::string& name = iter->first;
|
||||
const Any& a_in = iter->second;
|
||||
FromJson<E>(a_in, e);
|
||||
value[name] = std::move(e);
|
||||
}
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
void FromJson(const Any& a, T& t)
|
||||
{
|
||||
FromAny(a, t);
|
||||
}
|
||||
|
||||
void ToAny(const int32_t& value, Any& a);
|
||||
|
||||
void ToAny(const uint32_t& value, Any& a);
|
||||
|
||||
void ToAny(const int64_t& value, Any& a);
|
||||
|
||||
void ToAny(const uint64_t& value, Any& a);
|
||||
|
||||
void ToAny(const float& value, Any& a);
|
||||
|
||||
void ToAny(const double& value, Any& a);
|
||||
|
||||
void ToAny(const bool& value, Any& a);
|
||||
|
||||
void ToAny(const std::string& value, Any& a);
|
||||
|
||||
void ToAny(const JsonMap& value, Any& a);
|
||||
|
||||
void ToAny(const JsonArray& value, Any& a);
|
||||
|
||||
void ToAny(const Jsonize& value, Any& a);
|
||||
|
||||
template<typename E>
|
||||
void ToAny(const std::vector<E>& value, Any& a)
|
||||
{
|
||||
JsonArray arr;
|
||||
|
||||
size_t i, count = value.size();
|
||||
for (i=0; i<count; i++)
|
||||
{
|
||||
Any a_in;
|
||||
const E& e = value[i];
|
||||
|
||||
ToJson<E>(e, a_in);
|
||||
arr.push_back(std::move(a_in));
|
||||
}
|
||||
|
||||
a = Any(arr);
|
||||
}
|
||||
|
||||
template<typename E>
|
||||
void ToAny(const std::list<E>& value, Any& a)
|
||||
{
|
||||
JsonArray arr;
|
||||
|
||||
typename std::list<E>::const_iterator iter;
|
||||
for (iter = value.begin(); iter != value.end(); ++iter)
|
||||
{
|
||||
Any a_in;
|
||||
const E& e = *iter;
|
||||
|
||||
ToJson<E>(e, a_in);
|
||||
arr.push_back(std::move(a_in));
|
||||
}
|
||||
|
||||
a = Any(arr);
|
||||
}
|
||||
|
||||
template<typename E>
|
||||
void ToAny(const std::set<E>& value, Any& a)
|
||||
{
|
||||
JsonArray arr;
|
||||
|
||||
typename std::set<E>::const_iterator iter;
|
||||
for (iter = value.begin(); iter != value.end(); ++iter)
|
||||
{
|
||||
Any a_in;
|
||||
const E& e = *iter;
|
||||
|
||||
ToJson<E>(e, a_in);
|
||||
arr.push_back(std::move(a_in));
|
||||
}
|
||||
|
||||
a = Any(arr);
|
||||
}
|
||||
|
||||
template<typename E>
|
||||
void ToAny(const std::map<std::string,E>& value, Any& a)
|
||||
{
|
||||
JsonMap m;
|
||||
typename std::map<std::string,E>::const_iterator iter;
|
||||
|
||||
for (iter = value.begin(); iter != value.end(); ++iter)
|
||||
{
|
||||
Any a_in;
|
||||
const std::string& name = iter->first;
|
||||
const E& e = iter->second;
|
||||
|
||||
ToJson<E>(e, a_in);
|
||||
m[name] = a_in;
|
||||
}
|
||||
|
||||
a = Any(m);
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
void ToJson(const T& value, Any& a)
|
||||
{
|
||||
//std::cout << typeid(value).name() << std::endl;
|
||||
ToAny(value, a);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif//__UT_JSONIZE_HPP__
|
||||
|
|
@ -1,198 +0,0 @@
|
|||
#ifndef __UT_LOCK_HPP__
|
||||
#define __UT_LOCK_HPP__
|
||||
|
||||
#include <unitree/common/decl.hpp>
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
enum
|
||||
{
|
||||
UT_LOCK_MODE_READ = 0,
|
||||
UT_LOCK_MODE_WRITE = 1,
|
||||
UT_LOCK_MODE_UNLCK = 2
|
||||
};
|
||||
|
||||
enum
|
||||
{
|
||||
UT_LOCK_ENO_INTR = EINTR,
|
||||
UT_LOCK_ENO_BUSY = EBUSY,
|
||||
UT_LOCK_ENO_TIMEDOUT = ETIMEDOUT
|
||||
};
|
||||
|
||||
class Spinlock
|
||||
{
|
||||
public:
|
||||
explicit Spinlock();
|
||||
~Spinlock();
|
||||
|
||||
void Lock();
|
||||
void Unlock();
|
||||
|
||||
bool Trylock();
|
||||
|
||||
pthread_spinlock_t & GetNative();
|
||||
|
||||
private:
|
||||
pthread_spinlock_t mNative;
|
||||
};
|
||||
|
||||
class CaspinLock
|
||||
{
|
||||
public:
|
||||
explicit CaspinLock();
|
||||
~CaspinLock();
|
||||
|
||||
void Lock();
|
||||
void Unlock();
|
||||
|
||||
bool Trylock();
|
||||
|
||||
private:
|
||||
volatile int32_t mData;
|
||||
};
|
||||
|
||||
class Mutex
|
||||
{
|
||||
public:
|
||||
explicit Mutex();
|
||||
~Mutex();
|
||||
|
||||
void Lock();
|
||||
void Unlock();
|
||||
|
||||
bool Trylock();
|
||||
|
||||
pthread_mutex_t & GetNative();
|
||||
|
||||
private:
|
||||
pthread_mutex_t mNative;
|
||||
};
|
||||
|
||||
class Cond
|
||||
{
|
||||
public:
|
||||
explicit Cond();
|
||||
~Cond();
|
||||
|
||||
void Wait(Mutex& mutex);
|
||||
bool Wait(Mutex& mutex, uint64_t microsec);
|
||||
|
||||
void Notify();
|
||||
void NotifyAll();
|
||||
|
||||
private:
|
||||
pthread_cond_t mNative;
|
||||
};
|
||||
|
||||
class MutexCond
|
||||
{
|
||||
public:
|
||||
explicit MutexCond();
|
||||
~MutexCond();
|
||||
|
||||
void Lock();
|
||||
void Unlock();
|
||||
|
||||
bool Wait(int64_t microsec = 0);
|
||||
void Notify();
|
||||
void NotifyAll();
|
||||
|
||||
private:
|
||||
Mutex mMutex;
|
||||
Cond mCond;
|
||||
};
|
||||
|
||||
class Rwlock
|
||||
{
|
||||
public:
|
||||
explicit Rwlock();
|
||||
~Rwlock();
|
||||
|
||||
void Lock(int32_t mode);
|
||||
void Unlock();
|
||||
|
||||
bool Trylock(int32_t mode);
|
||||
|
||||
pthread_rwlock_t & GetNative();
|
||||
|
||||
private:
|
||||
pthread_rwlock_t mNative;
|
||||
};
|
||||
|
||||
class Filelock
|
||||
{
|
||||
public:
|
||||
explicit Filelock(const std::string& fileName);
|
||||
explicit Filelock(int32_t fd);
|
||||
~Filelock();
|
||||
|
||||
void Lock(int32_t mode, int64_t start = 0, int64_t len = 0);
|
||||
void Unlock();
|
||||
|
||||
bool Trylock(int32_t mode, int64_t start = 0, int64_t len = 0);
|
||||
|
||||
private:
|
||||
void SetLockMember(int32_t mode, int64_t start, int64_t len);
|
||||
void SetLockMode(int32_t mode);
|
||||
|
||||
private:
|
||||
int32_t mFd;
|
||||
bool mCloseFd;
|
||||
struct flock mLock;
|
||||
};
|
||||
|
||||
template<typename LOCK_TYPE>
|
||||
class LockGuard
|
||||
{
|
||||
public:
|
||||
explicit LockGuard(LOCK_TYPE& lockPtr)
|
||||
{
|
||||
mLockPtr = &lockPtr;
|
||||
mLockPtr->Lock();
|
||||
}
|
||||
|
||||
explicit LockGuard(LOCK_TYPE* lockPtr)
|
||||
{
|
||||
mLockPtr = lockPtr;
|
||||
mLockPtr->Lock();
|
||||
}
|
||||
|
||||
~LockGuard()
|
||||
{
|
||||
mLockPtr->Unlock();
|
||||
}
|
||||
|
||||
private:
|
||||
LOCK_TYPE* mLockPtr;
|
||||
};
|
||||
|
||||
template<typename LOCK_TYPE>
|
||||
class RwLockGuard
|
||||
{
|
||||
public:
|
||||
explicit RwLockGuard(LOCK_TYPE& lockPtr, int32_t mode)
|
||||
{
|
||||
mLockPtr = &lockPtr;
|
||||
lockPtr.Lock(mode);
|
||||
}
|
||||
|
||||
explicit RwLockGuard(LOCK_TYPE* lockPtr, int32_t mode)
|
||||
{
|
||||
mLockPtr = lockPtr;
|
||||
lockPtr->Lock(mode);
|
||||
}
|
||||
|
||||
~RwLockGuard()
|
||||
{
|
||||
mLockPtr->Unlock();
|
||||
}
|
||||
|
||||
private:
|
||||
LOCK_TYPE* mLockPtr;
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
#endif//__UT_LOCK_HPP__
|
||||
|
|
@ -1,6 +0,0 @@
|
|||
#ifndef __UT_LOG_HPP__
|
||||
#define __UT_LOG_HPP__
|
||||
|
||||
#include <unitree/common/log/log_initor.hpp>
|
||||
|
||||
#endif//__UT_LOG_HPP__
|
||||
|
|
@ -1,63 +0,0 @@
|
|||
#ifndef __UT_LOG_BUFFER_HPP__
|
||||
#define __UT_LOG_BUFFER_HPP__
|
||||
|
||||
#include <unitree/common/log/log_decl.hpp>
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
class LogBuffer
|
||||
{
|
||||
public:
|
||||
LogBuffer();
|
||||
|
||||
bool Append(const std::string& s);
|
||||
|
||||
const std::string& Get();
|
||||
void Get(std::string& s);
|
||||
|
||||
void Clear();
|
||||
|
||||
bool Empty();
|
||||
|
||||
private:
|
||||
std::string mData;
|
||||
};
|
||||
|
||||
typedef std::shared_ptr<LogBuffer> LogBufferPtr;
|
||||
|
||||
class LogBlockBuffer
|
||||
{
|
||||
public:
|
||||
typedef std::shared_ptr<LogBuffer> LOG_BUFFER_PTR;
|
||||
|
||||
LogBlockBuffer();
|
||||
~LogBlockBuffer();
|
||||
|
||||
bool Append(const std::string& s);
|
||||
|
||||
/*
|
||||
* Get reference of data. [UNSAFE]
|
||||
*/
|
||||
const std::string& Get();
|
||||
void Get(std::string& s);
|
||||
|
||||
/*
|
||||
* Clear data.
|
||||
*/
|
||||
void Clear(bool lock = false);
|
||||
|
||||
void Exchange();
|
||||
|
||||
private:
|
||||
volatile bool mR;
|
||||
std::vector<LOG_BUFFER_PTR> mChain;
|
||||
Mutex mLock;
|
||||
};
|
||||
|
||||
typedef std::shared_ptr<LogBlockBuffer> LogBlockBufferPtr;
|
||||
}
|
||||
}
|
||||
|
||||
#endif//__UT_LOG_BUFFER_HPP__
|
||||
|
|
@ -1,248 +0,0 @@
|
|||
#ifndef __UT_LOG_DECL_HPP__
|
||||
#define __UT_LOG_DECL_HPP__
|
||||
|
||||
#include <unitree/common/exception.hpp>
|
||||
#include <unitree/common/os.hpp>
|
||||
#include <unitree/common/lock/lock.hpp>
|
||||
#include <unitree/common/thread/thread.hpp>
|
||||
#include <unitree/common/thread/recurrent_thread.hpp>
|
||||
#include <unitree/common/filesystem/file.hpp>
|
||||
|
||||
/*
|
||||
* log buffer size
|
||||
*/
|
||||
#define UT_LOG_BUFFER_SIZE 65535 //64K
|
||||
#define UT_LOG_MAX_BUFFER_SIZE 8388608 //8M
|
||||
/*
|
||||
* log file size
|
||||
*/
|
||||
#define UT_LOG_FILE_SIZE 104857600 //100M
|
||||
#define UT_LOG_MAX_FILE_SIZE 10737418240 //10G
|
||||
#define UT_LOG_MIN_FILE_SIZE UT_LOG_MAX_BUFFER_SIZE
|
||||
|
||||
/*
|
||||
* log file number
|
||||
*/
|
||||
#define UT_LOG_FILE_NUMBER 10
|
||||
#define UT_LOG_MAX_FILE_NUMBER 1000
|
||||
|
||||
#define UT_LOG_FILE_EXT ".LOG"
|
||||
|
||||
//write log macro wrapper
|
||||
#define __UT_LOG(logger, level, ...)\
|
||||
do { \
|
||||
if (logger != NULL) \
|
||||
{ \
|
||||
logger->Log(level, __VA_ARGS__); \
|
||||
} \
|
||||
} while (0)
|
||||
|
||||
#define __UT_CRIT_LOG(logger, key, code, ...) \
|
||||
do { \
|
||||
if (logger != NULL) \
|
||||
{ \
|
||||
logger->CritLog(UT_LOG_CRIT, key, code, __VA_ARGS__);\
|
||||
} \
|
||||
} while (0)
|
||||
|
||||
|
||||
//debug
|
||||
#define LOG_DEBUG(logger, ...) \
|
||||
__UT_LOG(logger, UT_LOG_DEBUG, __VA_ARGS__)
|
||||
|
||||
//info
|
||||
#define LOG_INFO(logger, ...) \
|
||||
__UT_LOG(logger, UT_LOG_INFO, __VA_ARGS__)
|
||||
|
||||
//warning
|
||||
#define LOG_WARNING(logger, ...) \
|
||||
__UT_LOG(logger, UT_LOG_WARNING, __VA_ARGS__)
|
||||
|
||||
//error
|
||||
#define LOG_ERROR(logger, ...) \
|
||||
__UT_LOG(logger, UT_LOG_ERROR, __VA_ARGS__)
|
||||
|
||||
//fatal
|
||||
#define LOG_FATAL(logger, ...) \
|
||||
__UT_LOG(logger, UT_LOG_FATAL, __VA_ARGS__)
|
||||
|
||||
//critical log. the 1st args is CRITICAL KEY
|
||||
#define CRIT_LOG(logger, ...) \
|
||||
__UT_CRIT_LOG(logger, __VA_ARGS__)
|
||||
|
||||
#define CRIT_LOG(logger, ...) \
|
||||
__UT_CRIT_LOG(logger, __VA_ARGS__)
|
||||
|
||||
//write log format macro wrapper
|
||||
/*
|
||||
* FMT_DEBUG(logger, ("key1", val1)("key2", val2)("keyn", ""));
|
||||
*/
|
||||
#define __UT_LOG_FMT(logger, level, keyvalues) \
|
||||
do { \
|
||||
if (logger != NULL) \
|
||||
{ \
|
||||
logger->LogFormat(level, unitree::common::LogBuilder() keyvalues); \
|
||||
} \
|
||||
} while (0)
|
||||
|
||||
#define __UT_CRIT_LOG_FMT(logger, key, code, keyvalues) \
|
||||
do { \
|
||||
if (logger != NULL) \
|
||||
{ \
|
||||
logger->CritLogFormat(UT_LOG_CRIT, key, code, unitree::common::LogBuilder() keyvalues); \
|
||||
} \
|
||||
} while (0)
|
||||
|
||||
//debug
|
||||
#define FMT_DEBUG(logger, keyvalues) \
|
||||
__UT_LOG_FMT(logger, UT_LOG_DEBUG, keyvalues)
|
||||
|
||||
//info
|
||||
#define FMT_INFO(logger, keyvalues) \
|
||||
__UT_LOG_FMT(logger, UT_LOG_INFO, keyvalues)
|
||||
|
||||
//warning
|
||||
#define FMT_WARNING(logger, keyvalues) \
|
||||
__UT_LOG_FMT(logger, UT_LOG_WARNING, keyvalues)
|
||||
|
||||
//error
|
||||
#define FMT_ERROR(logger, keyvalues) \
|
||||
__UT_LOG_FMT(logger, UT_LOG_ERROR, keyvalues)
|
||||
|
||||
//fatal
|
||||
#define FMT_FATAL(logger, keyvalues) \
|
||||
__UT_LOG_FMT(logger, UT_LOG_FATAL, keyvalues)
|
||||
|
||||
#define CRIT_FMT(logger, critkey, keyvalues) \
|
||||
__UT_CRIT_LOG_FMT(logger, critkey, keyvalues)
|
||||
|
||||
|
||||
/*
|
||||
//declare and define log level
|
||||
#define UT_LOG_DECL_LEVEL(name, level, desc) \
|
||||
const int32_t name = level; \
|
||||
const std::string name##_DESC = desc;
|
||||
#define UT_LOG_DESC_LEVEL(name) name##_DESC
|
||||
*/
|
||||
|
||||
//declare and define log store type
|
||||
#define UT_LOG_DECL_STORE_TYPE(name, type, desc)\
|
||||
const int32_t name = type; \
|
||||
const std::string name##_DESC = desc;
|
||||
#define UT_LOG_DESC_STORE_TYPE(name) name##_DESC
|
||||
|
||||
//define log level
|
||||
#define UT_LOG_NONE 0
|
||||
#define UT_LOG_CRIT 1
|
||||
#define UT_LOG_FATAL 2
|
||||
#define UT_LOG_ERROR 3
|
||||
#define UT_LOG_WARNING 4
|
||||
#define UT_LOG_INFO 5
|
||||
#define UT_LOG_DEBUG 6
|
||||
#define UT_LOG_ALL 7
|
||||
|
||||
#define UT_LOG_DESC_NONE "NONE"
|
||||
#define UT_LOG_DESC_CRIT "CRIT"
|
||||
#define UT_LOG_DESC_FATAL "FATAL"
|
||||
#define UT_LOG_DESC_ERROR "ERROR"
|
||||
#define UT_LOG_DESC_WARNING "WARNING"
|
||||
#define UT_LOG_DESC_INFO "INFO"
|
||||
#define UT_LOG_DESC_DEBUG "DEBUG"
|
||||
#define UT_LOG_DESC_ALL "ALL"
|
||||
|
||||
//define log store type
|
||||
#define UT_LOG_STORE_FILE_ASYNC 0
|
||||
#define UT_LOG_STORE_FILE 1
|
||||
#define UT_LOG_STORE_STDOUT 2
|
||||
#define UT_LOG_STORE_STDERR 3
|
||||
|
||||
#define UT_LOG_STORE_DESC_FILE_ASYNC "FILEASYNC"
|
||||
#define UT_LOG_STORE_DESC_FILE "FILE"
|
||||
#define UT_LOG_STORE_DESC_STDOUT "STDOUT"
|
||||
#define UT_LOG_STORE_DESC_STDERR "STDERR"
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
static inline int32_t GetLogLevel(const std::string& desc)
|
||||
{
|
||||
if (desc == UT_LOG_DESC_NONE) {
|
||||
return UT_LOG_NONE; }
|
||||
else if (desc == UT_LOG_DESC_CRIT) {
|
||||
return UT_LOG_CRIT; }
|
||||
else if (desc == UT_LOG_DESC_FATAL) {
|
||||
return UT_LOG_FATAL; }
|
||||
else if (desc == UT_LOG_DESC_ERROR) {
|
||||
return UT_LOG_ERROR; }
|
||||
else if (desc == UT_LOG_DESC_WARNING) {
|
||||
return UT_LOG_WARNING; }
|
||||
else if (desc == UT_LOG_DESC_INFO) {
|
||||
return UT_LOG_INFO; }
|
||||
else if (desc == UT_LOG_DESC_DEBUG) {
|
||||
return UT_LOG_DEBUG; }
|
||||
else if (desc == UT_LOG_DESC_ALL) {
|
||||
return UT_LOG_ALL; }
|
||||
|
||||
UT_THROW(CommonException, std::string("unknown log level desc:") + desc);
|
||||
}
|
||||
|
||||
static inline const char* GetLogLevelDesc(int32_t level)
|
||||
{
|
||||
switch (level)
|
||||
{
|
||||
case UT_LOG_NONE:
|
||||
return UT_LOG_DESC_NONE;
|
||||
case UT_LOG_CRIT:
|
||||
return UT_LOG_DESC_CRIT;
|
||||
case UT_LOG_FATAL:
|
||||
return UT_LOG_DESC_FATAL;
|
||||
case UT_LOG_ERROR:
|
||||
return UT_LOG_DESC_ERROR;
|
||||
case UT_LOG_WARNING:
|
||||
return UT_LOG_DESC_WARNING;
|
||||
case UT_LOG_INFO:
|
||||
return UT_LOG_DESC_INFO;
|
||||
case UT_LOG_DEBUG:
|
||||
return UT_LOG_DESC_DEBUG;
|
||||
case UT_LOG_ALL:
|
||||
return UT_LOG_DESC_ALL;
|
||||
}
|
||||
|
||||
UT_THROW(CommonException, "unknown log level");
|
||||
}
|
||||
|
||||
static inline int32_t GetLogStoreType(const std::string& desc)
|
||||
{
|
||||
if (desc == UT_LOG_STORE_DESC_FILE_ASYNC) {
|
||||
return UT_LOG_STORE_FILE_ASYNC; }
|
||||
else if (desc == UT_LOG_STORE_DESC_FILE) {
|
||||
return UT_LOG_STORE_FILE; }
|
||||
else if (desc == UT_LOG_STORE_DESC_STDOUT){
|
||||
return UT_LOG_STORE_STDOUT; }
|
||||
else if (desc == UT_LOG_STORE_DESC_STDERR){
|
||||
return UT_LOG_STORE_STDERR; }
|
||||
|
||||
UT_THROW(CommonException, std::string("unknown log store type desc:") + desc);
|
||||
}
|
||||
|
||||
static inline const char* GetLogStoreTypeDesc(int32_t type)
|
||||
{
|
||||
switch (type)
|
||||
{
|
||||
case UT_LOG_STORE_FILE_ASYNC:
|
||||
return UT_LOG_STORE_DESC_FILE_ASYNC;
|
||||
case UT_LOG_STORE_FILE:
|
||||
return UT_LOG_STORE_DESC_FILE;
|
||||
case UT_LOG_STORE_STDOUT:
|
||||
return UT_LOG_STORE_DESC_STDOUT;
|
||||
case UT_LOG_STORE_STDERR:
|
||||
return UT_LOG_STORE_DESC_STDERR;
|
||||
}
|
||||
|
||||
UT_THROW(CommonException, "unknown log store type");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif//__UT_LOG_DECL_HPP__
|
||||
|
|
@ -1,48 +0,0 @@
|
|||
#ifndef __UT_LOG_INITOR_HPP__
|
||||
#define __UT_LOG_INITOR_HPP__
|
||||
|
||||
#include <unitree/common/log/log_policy.hpp>
|
||||
#include <unitree/common/log/log_store.hpp>
|
||||
#include <unitree/common/log/log_logger.hpp>
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
class LogInitor
|
||||
{
|
||||
public:
|
||||
static LogInitor* Instance()
|
||||
{
|
||||
static LogInitor inst;
|
||||
return &inst;
|
||||
}
|
||||
|
||||
void Init(const std::string& configFileName);
|
||||
Logger* GetLogger(const std::string& tag);
|
||||
|
||||
void Final();
|
||||
|
||||
private:
|
||||
LogInitor();
|
||||
void ParseConf(Any json);
|
||||
void InitLogger();
|
||||
|
||||
private:
|
||||
bool mInited;
|
||||
std::set<std::string> mStoreNames;
|
||||
std::vector<LogPolicyPtr> mPolicis;
|
||||
std::vector<LogStorePolicyPtr> mStorePolicis;
|
||||
std::map<std::string, LoggerPtr> mLoggerMap;
|
||||
Mutex mLock;
|
||||
};
|
||||
|
||||
void LogInit(const std::string& configFileName = "");
|
||||
void LogFinal();
|
||||
|
||||
Logger* GetLogger(const std::string& tag);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
#endif//__UT_LOG_INITOR_HPP__
|
||||
|
|
@ -1,56 +0,0 @@
|
|||
#ifndef __UT_LOG_FILE_KEEPER_H__
|
||||
#define __UT_LOG_FILE_KEEPER_H__
|
||||
|
||||
#include <unitree/common/log/log_policy.hpp>
|
||||
#include <unitree/common/log/log_writer.hpp>
|
||||
|
||||
namespace unitree
|
||||
{
|
||||
namespace common
|
||||
{
|
||||
class LogKeeper
|
||||
{
|
||||
public:
|
||||
enum
|
||||
{
|
||||
ROLLING_WAIT_MICROSEC = 1000000
|
||||
};
|
||||
|
||||
LogKeeper(LogStorePolicyPtr storePolicyPtr);
|
||||
~LogKeeper();
|
||||
|
||||
void SetWriter(LogWriterPtr writerPtr);
|
||||
void AppendDataSize(int64_t len);
|
||||
|
||||
private:
|
||||
void Rolling();
|
||||
|
||||
void OpenFile();
|
||||
void CloseFile();
|
||||
bool CheckFile();
|
||||
|
||||
void ThreadRolling();
|
||||
|
||||
bool IsNeedToRolling(int64_t len);
|
||||
void CheckRolling();
|
||||
|
||||
std::string MakeRegexExpress();
|
||||
|
||||
private:
|
||||
bool mQuit;
|
||||
int64_t mFileSize;
|
||||
std::string mFileName;
|
||||
std::string mDirectory;
|
||||
FilePtr mFilePtr;
|
||||
LogStorePolicyPtr mStorePolicyPtr;
|
||||
LogWriterPtr mWriterPtr;
|
||||
ThreadPtr mThreadPtr;
|
||||
MutexCond mMutexCond;
|
||||
};
|
||||
|
||||
typedef std::shared_ptr<LogKeeper> LogKeeperPtr;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
#endif//__UT_LOG_FILE_KEEPER_H__
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue