diff --git a/.dockerignore b/.dockerignore new file mode 100644 index 0000000..fd00eaa --- /dev/null +++ b/.dockerignore @@ -0,0 +1,34 @@ +# Include any files or directories that you don't want to be copied to your +# container here (e.g., local build artifacts, temporary files, etc.). +# +# For more help, visit the .dockerignore file reference guide at +# https://docs.docker.com/go/build-context-dockerignore/ + +**/.classpath +**/.dockerignore +**/.env +**/.git +**/.gitignore +**/.project +**/.settings +**/.toolstarget +**/.vs +**/.vscode +**/.next +**/.cache +**/*.*proj.user +**/*.dbmdl +**/*.jfm +**/charts +**/docker-compose* +**/compose* +**/Dockerfile* +**/node_modules +**/npm-debug.log +**/obj +**/secrets.dev.yaml +**/values.dev.yaml +**/build +**/dist +LICENSE +README.md diff --git a/CMakeLists.txt b/CMakeLists.txt index de19b93..f8238bf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,6 +12,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) ########################################################################## add_subdirectory(external/107-Arduino-Cyphal) @@ -29,7 +30,7 @@ add_executable(${VIPER_TARGET} target_link_libraries(${VIPER_TARGET} cyphal++ socketcan mp-units) ####################################################################################### target_compile_features(${VIPER_TARGET} PRIVATE cxx_std_20) -ament_target_dependencies(${VIPER_TARGET} rclcpp std_msgs geometry_msgs) +ament_target_dependencies(${VIPER_TARGET} rclcpp std_msgs sensor_msgs geometry_msgs) ####################################################################################### install(TARGETS ${VIPER_TARGET} DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..a8ed07d --- /dev/null +++ b/Dockerfile @@ -0,0 +1,38 @@ +# syntax=docker/dockerfile:1 + +# Comments are provided throughout this file to help you get started. +# If you need more help, visit the Dockerfile reference guide at +# https://docs.docker.com/go/dockerfile-reference/ + +# Want to help us make this template better? Share your feedback here: https://forms.gle/ybq9Krt8jtBL3iCk7 + +ARG NODE_VERSION=Viper + +FROM node:${NODE_VERSION}-alpine + +# Use production node environment by default. +ENV NODE_ENV production + + +WORKDIR /usr/src/app + +# Download dependencies as a separate step to take advantage of Docker's caching. +# Leverage a cache mount to /root/.npm to speed up subsequent builds. +# Leverage a bind mounts to package.json and package-lock.json to avoid having to copy them into +# into this layer. +RUN --mount=type=bind,source=package.json,target=package.json \ + --mount=type=bind,source=package-lock.json,target=package-lock.json \ + --mount=type=cache,target=/root/.npm \ + npm ci --omit=dev + +# Run the application as a non-root user. +USER node + +# Copy the rest of the source files into the image. +COPY . . + +# Expose the port that the application listens on. +EXPOSE 8088 + +# Run the application. +CMD ros2 launch viper viper-quad.py diff --git a/README.Docker.md b/README.Docker.md new file mode 100644 index 0000000..2e72177 --- /dev/null +++ b/README.Docker.md @@ -0,0 +1,22 @@ +### Building and running your application + +When you're ready, start your application by running: +`docker compose up --build`. + +Your application will be available at http://localhost:8088. + +### Deploying your application to the cloud + +First, build your image, e.g.: `docker build -t myapp .`. +If your cloud uses a different CPU architecture than your development +machine (e.g., you are on a Mac M1 and your cloud provider is amd64), +you'll want to build the image for that platform, e.g.: +`docker build --platform=linux/amd64 -t myapp .`. + +Then, push it to your registry, e.g. `docker push myregistry.com/myapp`. + +Consult Docker's [getting started](https://docs.docker.com/go/get-started-sharing/) +docs for more detail on building and pushing. + +### References +* [Docker's Node.js guide](https://docs.docker.com/language/nodejs/) \ No newline at end of file diff --git a/README.md b/README.md index ec75e9b..292c2fe 100644 --- a/README.md +++ b/README.md @@ -13,16 +13,19 @@ Generic ROS based drone flight stack for the [Pika Spark](https://pika-spark.io/ #### How-to-build Note: Don't forget to install the [dependencies](https://github.com/107-systems/viper#install-dependencies) (see at the end of this file). ```bash -cd $COLCON_WS/src +cd my_colcon_ws/src git clone --recursive https://github.com/107-systems/viper -cd $COLCON_WS +cd my_colcon_ws +cd Documents/DEV/src +git clone --recursive https://github.com/inusha47/viper-WOP.git +cd Documents/DEV source /opt/ros/humble/setup.bash colcon build --packages-select viper ``` #### How-to-run ```bash -cd $COLCON_WS +cd my_colcon_ws . install/setup.bash ros2 launch viper viper-quad.py ``` diff --git a/compose.yaml b/compose.yaml new file mode 100644 index 0000000..ab6d3db --- /dev/null +++ b/compose.yaml @@ -0,0 +1,51 @@ +# Comments are provided throughout this file to help you get started. +# If you need more help, visit the Docker compose reference guide at +# https://docs.docker.com/go/compose-spec-reference/ + +# Here the instructions define your application as a service called "server". +# This service is built from the Dockerfile in the current directory. +# You can add other services your application may depend on here, such as a +# database or a cache. For examples, see the Awesome Compose repository: +# https://github.com/docker/awesome-compose +services: + server: + build: + context: . + environment: + NODE_ENV: production + ports: + - 8088:8088 + +# The commented out section below is an example of how to define a PostgreSQL +# database that your application can use. `depends_on` tells Docker Compose to +# start the database before your application. The `db-data` volume persists the +# database data between container restarts. The `db-password` secret is used +# to set the database password. You must create `db/password.txt` and add +# a password of your choosing to it before running `docker-compose up`. +# depends_on: +# db: +# condition: service_healthy +# db: +# image: postgres +# restart: always +# user: postgres +# secrets: +# - db-password +# volumes: +# - db-data:/var/lib/postgresql/data +# environment: +# - POSTGRES_DB=example +# - POSTGRES_PASSWORD_FILE=/run/secrets/db-password +# expose: +# - 5432 +# healthcheck: +# test: [ "CMD", "pg_isready" ] +# interval: 10s +# timeout: 5s +# retries: 5 +# volumes: +# db-data: +# secrets: +# db-password: +# file: db/password.txt + diff --git a/include/viper/Node.h b/include/viper/Node.h index 702dd14..77f8420 100644 --- a/include/viper/Node.h +++ b/include/viper/Node.h @@ -1,4 +1,4 @@ -/** + /** * Copyright (c) 2023 LXRobotics GmbH. * Author: Alexander Entinger * Contributors: https://github.com/107-systems/viper/graphs/contributors. @@ -16,6 +16,7 @@ #include +#include #include #include @@ -76,9 +77,35 @@ class Node : public rclcpp::Node quantity _target_angular_velocity_x, _target_angular_velocity_y, _target_angular_velocity_z; void init_teleop_sub(); + rclcpp::QoS _imu_qos_profile; + rclcpp::SubscriptionOptions _imu_sub_options; + rclcpp::Subscription::SharedPtr _imu_sub; + sensor_msgs::msg::Imu _imu_data; + void init_imu_sub(); + + static uint16_t constexpr CYPHAL_DEMO_PORT_ID = 1234; cyphal::Publisher _cyphal_demo_pub; + static uint16_t constexpr SETPOINT_VELOCITY_ID_1 = 113; + cyphal::Publisher _setpoint_velocity_pub_1; + + static uint16_t constexpr SETPOINT_VELOCITY_ID_2 = 114; + cyphal::Publisher _setpoint_velocity_pub_2; + + + static uint16_t constexpr SETPOINT_VELOCITY_ID_3 = 115; + cyphal::Publisher _setpoint_velocity_pub_3; + + static uint16_t constexpr SETPOINT_VELOCITY_ID_4 = 116; + cyphal::Publisher _setpoint_velocity_pub_4; + + + + + + + static std::chrono::milliseconds constexpr CTRL_LOOP_RATE{10}; rclcpp::TimerBase::SharedPtr _ctrl_loop_timer; void ctrl_loop(); @@ -88,4 +115,4 @@ class Node : public rclcpp::Node * NAMESPACE **************************************************************************************/ -} /* viper */ +} /* viper */ \ No newline at end of file diff --git a/launch/viper-quad.py b/launch/viper-quad.py index b857c87..d9e9d9f 100644 --- a/launch/viper-quad.py +++ b/launch/viper-quad.py @@ -1,21 +1,31 @@ from launch import LaunchDescription +from launch.actions import ExecuteProcess from launch_ros.actions import Node def generate_launch_description(): - return LaunchDescription([ - Node( - package='viper', - executable='viper_node', - name='viper', - namespace='viper', - output='screen', - emulate_tty=True, - parameters=[ - {'can_iface' : 'can0'}, - {'can_node_id' : 100}, - {'teleop_topic': 'cmd_vel'}, - {'teleop_topic_deadline_ms': 100}, - {'teleop_topic_liveliness_lease_duration': 1000}, - ] - ) - ]) + return LaunchDescription([ + Node( + package='viper', + executable='viper_node', + name='viper', + namespace='viper', + output='screen', + emulate_tty=True, + parameters=[ + {'can_iface': 'can0'}, + {'can_node_id': 100}, + {'teleop_topic': 'cmd_vel'}, + {'teleop_topic_deadline_ms': 100}, + {'teleop_topic_liveliness_lease_duration': 1000}, + {'imu_topic': '/imu'}, + {'imu_topic_deadline_ms': 100}, + {'imu_topic_liveliness_lease_duration': 1000}, + ] + ), + ExecuteProcess( + cmd=['ros2', 'topic', 'echo', 'imu'], + output='screen', + additional_env={'period': '0.1', 'msg': '[100, 100, 100, 100]', 'topic': '113:zubax.primitive.real16.Vector4'} + ), + + ]) diff --git a/package.xml b/package.xml index 048edbe..e71f281 100644 --- a/package.xml +++ b/package.xml @@ -11,6 +11,7 @@ rclcpp std_msgs + sensor_msgs geometry_msgs ament_lint_auto diff --git a/prac.yaml b/prac.yaml new file mode 100644 index 0000000..b17fb16 --- /dev/null +++ b/prac.yaml @@ -0,0 +1,397 @@ +122: {} +123: + aux.ctl.1_rcpwm.deadband: 9.999999747378752e-05 + aux.ctl.1_rcpwm.rev_mid_fwd: [0.0010000000474974513, 0.001500000013038516, 0.0020000000949949026] + aux.ctl.2_voltage.deadband: 0.15000000596046448 + aux.ctl.2_voltage.rev_mid_fwd: [0.0, 1.649999976158142, 3.299999952316284] + aux.ctl.dead0: false + aux.ctl.mode: '' + aux.ctl.type: 0 + aux.power_output: false + aux.pull: 1 + drive.current_brake_pu: 0.20000000298023224 + drive.flux_weakening.bemf_pid: [4.0, 2.0, 0.009999999776482582] + drive.flux_weakening.voltage_boost: 1.0 + drive.observer.0_ekf.proc_noise: [1000000.0, 3000000.0, 100000000.0] + drive.observer.0_ekf.sched_factor: [0.800000011920929, 0.949999988079071] + drive.observer.1_mras.gain: 1000.0 + drive.observer.type: 0 + drive.pre_calibrate: false + drive.runner.0_ramp.spinup_current_pu: [0.5, 0.5] + drive.runner.0_ramp.spinup_duration: [0.20000000298023224, 0.4000000059604645, 1.5, 0.0020000000949949026, 0.014999999664723873, 0.6000000238418579] + drive.runner.0_ramp.velocity_stall_spinup: [5.0, 20.0] + drive.runner.1_passive.delay: 0.20000000298023224 + drive.runner.1_passive.velocity_off_on: [10.0, 20.0] + drive.runner.type: 0 + drive.stall_limit: 20 + drive.velocity_ctl.1_pid.feedforward: [0.0, 0.0] + drive.velocity_ctl.1_pid.gain: [0.0, 0.0, 0.0] + drive.velocity_ctl.2_indi.acceleration_pi: [50.0, 0.0] + drive.velocity_ctl.2_indi.mass: 9.999999747378752e-06 + drive.velocity_ctl.acceleration: [-2000.0, 3000.0] + drive.velocity_ctl.type: 2 + drive.voltage_clipping: false + mns.deadman_timeout: 0.5 + mns.local_timestamp: false + mns.pub_interval_min: 0.05000000074505806 + mns.ratiometric_setpoint_min: 0.0 + mns.ratiometric_to_absolute_mul: 0.0 + mns.setpoint_index: 0 + mns.status_period: 1.0 + motor.current_ctl_bwr: 0.05000000074505806 + motor.current_max: 59.0 + motor.current_ramp: 5000.0 + motor.flux_linkage: 0.0008227403741329908 + motor.inductance_dq: [1.5992691260180436e-06, 1.5992691260180436e-06] + motor.mechanical_ratio: 12 + motor.resistance: 0.011695253662765026 + motor.thermistor_v2k: [0.0, 0.0, 0.0] + motor.voltage_ramp: 20.0 + servo.profile.acceleration: [-200.0, 100.0] + servo.profile.jerk: 2000.0 + servo.profile.velocity: [-50.0, 50.0] + standby.brake_voltage: .nan + sys.debug: false + sys.golden: sys. vsi. + uavcan.can.bitrate: [1000000, 1000000] + uavcan.can.count: 1 + uavcan.node.description: '' + uavcan.node.id: 123 + uavcan.pub.compact.id: 65535 + uavcan.pub.compact.prio: 3 + uavcan.pub.dq.id: 65535 + uavcan.pub.dq.prio: 3 + uavcan.pub.dynamics.id: 100 + uavcan.pub.dynamics.prio: 3 + uavcan.pub.feedback.id: 102 + uavcan.pub.feedback.prio: 3 + uavcan.pub.power.id: 101 + uavcan.pub.power.prio: 3 + uavcan.pub.status.id: 65535 + uavcan.pub.status.prio: 4 + uavcan.pub.temperature.id: 65535 + uavcan.pub.temperature.prio: 5 + uavcan.srv.low_level_io.id: 65535 + uavcan.sub.readiness.id: 65535 + uavcan.sub.setpoint_dynamics.id: 65535 + uavcan.sub.setpoint_rat_torque.id: 111 + uavcan.sub.setpoint_rat_torque_u9.id: 110 + uavcan.sub.setpoint_rat_velocity_u9.id: 65535 + uavcan.sub.setpoint_rat_voltage.id: 112 + uavcan.sub.setpoint_rat_voltage_u9.id: 65535 + uavcan.sub.setpoint_servo.id: 65535 + uavcan.sub.setpoint_velocity.id: 113 + vsi.activation_latency: 0.009999999776482582 + vsi.bridge_resistance: [0.0020000000949949026, 0.003000000026077032, 0.0020000000949949026, 0.003000000026077032, 0.0020000000949949026, 0.0020000000949949026] + vsi.calibration_duration: 2.0 + vsi.dc_voltage_gain: 18.799999237060547 + vsi.hw_fault_latency: 0.10000000149011612 + vsi.phase_current_gain: [100.0, 100.0, 25.0, 25.0] + vsi.phase_current_gain_attack_decay: [1.5, 0.36000001430511475] + vsi.phase_current_gain_decay_time: 0.10000000149011612 + vsi.phase_current_sampling_window: 1.9000000293090125e-06 + vsi.phase_current_stderr: [0.30000001192092896, 0.30000001192092896, 0.20000000298023224, 0.20000000298023224] + vsi.pwm_dead_time: 0.0 + vsi.pwm_freq_mul_log2: 0 + vsi.shortest_time_in_disabled_state: 1.9999999494757503e-05 + vsi.swap_ab: false + vsi.thermistor_v2k: [223.14999389648438, 100.0, 0.0] + vsi.tick_freq: 29402.15625 + zubax.cookie: '' +124: + aux.ctl.1_rcpwm.deadband: 9.999999747378752e-05 + aux.ctl.1_rcpwm.rev_mid_fwd: [0.0010000000474974513, 0.001500000013038516, 0.0020000000949949026] + aux.ctl.2_voltage.deadband: 0.15000000596046448 + aux.ctl.2_voltage.rev_mid_fwd: [0.0, 1.649999976158142, 3.299999952316284] + aux.ctl.dead0: false + aux.ctl.mode: '' + aux.ctl.type: 0 + aux.power_output: false + aux.pull: 1 + drive.current_brake_pu: 0.20000000298023224 + drive.flux_weakening.bemf_pid: [4.0, 2.0, 0.009999999776482582] + drive.flux_weakening.voltage_boost: 1.0 + drive.observer.0_ekf.proc_noise: [1000000.0, 3000000.0, 100000000.0] + drive.observer.0_ekf.sched_factor: [0.800000011920929, 0.949999988079071] + drive.observer.1_mras.gain: 1000.0 + drive.observer.type: 0 + drive.pre_calibrate: false + drive.runner.0_ramp.spinup_current_pu: [0.5, 0.5] + drive.runner.0_ramp.spinup_duration: [0.20000000298023224, 0.4000000059604645, 1.5, 0.0020000000949949026, 0.014999999664723873, 0.6000000238418579] + drive.runner.0_ramp.velocity_stall_spinup: [5.0, 20.0] + drive.runner.1_passive.delay: 0.20000000298023224 + drive.runner.1_passive.velocity_off_on: [10.0, 20.0] + drive.runner.type: 0 + drive.stall_limit: 20 + drive.velocity_ctl.1_pid.feedforward: [0.0, 0.0] + drive.velocity_ctl.1_pid.gain: [0.0, 0.0, 0.0] + drive.velocity_ctl.2_indi.acceleration_pi: [50.0, 0.0] + drive.velocity_ctl.2_indi.mass: 9.999999747378752e-06 + drive.velocity_ctl.acceleration: [-2000.0, 3000.0] + drive.velocity_ctl.type: 2 + drive.voltage_clipping: false + mns.deadman_timeout: 0.5 + mns.local_timestamp: false + mns.pub_interval_min: 0.05000000074505806 + mns.ratiometric_setpoint_min: 0.0 + mns.ratiometric_to_absolute_mul: 0.0 + mns.setpoint_index: 0 + mns.status_period: 1.0 + motor.current_ctl_bwr: 0.05000000074505806 + motor.current_max: 59.0 + motor.current_ramp: 5000.0 + motor.flux_linkage: 0.0008264124044217169 + motor.inductance_dq: [1.621233764126373e-06, 1.621233764126373e-06] + motor.mechanical_ratio: 12 + motor.resistance: 0.01173351239413023 + motor.thermistor_v2k: [0.0, 0.0, 0.0] + motor.voltage_ramp: 20.0 + servo.profile.acceleration: [-200.0, 100.0] + servo.profile.jerk: 2000.0 + servo.profile.velocity: [-50.0, 50.0] + standby.brake_voltage: .nan + sys.debug: false + sys.golden: sys. vsi. + uavcan.can.bitrate: [1000000, 1000000] + uavcan.can.count: 1 + uavcan.node.description: '' + uavcan.node.id: 124 + uavcan.pub.compact.id: 65535 + uavcan.pub.compact.prio: 3 + uavcan.pub.dq.id: 65535 + uavcan.pub.dq.prio: 3 + uavcan.pub.dynamics.id: 100 + uavcan.pub.dynamics.prio: 3 + uavcan.pub.feedback.id: 102 + uavcan.pub.feedback.prio: 3 + uavcan.pub.power.id: 101 + uavcan.pub.power.prio: 3 + uavcan.pub.status.id: 65535 + uavcan.pub.status.prio: 4 + uavcan.pub.temperature.id: 65535 + uavcan.pub.temperature.prio: 5 + uavcan.srv.low_level_io.id: 65535 + uavcan.sub.readiness.id: 65535 + uavcan.sub.setpoint_dynamics.id: 65535 + uavcan.sub.setpoint_rat_torque.id: 111 + uavcan.sub.setpoint_rat_torque_u9.id: 110 + uavcan.sub.setpoint_rat_velocity_u9.id: 65535 + uavcan.sub.setpoint_rat_voltage.id: 112 + uavcan.sub.setpoint_rat_voltage_u9.id: 65535 + uavcan.sub.setpoint_servo.id: 65535 + uavcan.sub.setpoint_velocity.id: 114 + vsi.activation_latency: 0.009999999776482582 + vsi.bridge_resistance: [0.0020000000949949026, 0.003000000026077032, 0.0020000000949949026, 0.003000000026077032, 0.0020000000949949026, 0.0020000000949949026] + vsi.calibration_duration: 2.0 + vsi.dc_voltage_gain: 18.799999237060547 + vsi.hw_fault_latency: 0.10000000149011612 + vsi.phase_current_gain: [100.0, 100.0, 25.0, 25.0] + vsi.phase_current_gain_attack_decay: [1.5, 0.36000001430511475] + vsi.phase_current_gain_decay_time: 0.10000000149011612 + vsi.phase_current_sampling_window: 1.9000000293090125e-06 + vsi.phase_current_stderr: [0.30000001192092896, 0.30000001192092896, 0.20000000298023224, 0.20000000298023224] + vsi.pwm_dead_time: 0.0 + vsi.pwm_freq_mul_log2: 0 + vsi.shortest_time_in_disabled_state: 1.9999999494757503e-05 + vsi.swap_ab: false + vsi.thermistor_v2k: [223.14999389648438, 100.0, 0.0] + vsi.tick_freq: 29402.15625 + zubax.cookie: '' +125: + aux.ctl.1_rcpwm.deadband: 9.999999747378752e-05 + aux.ctl.1_rcpwm.rev_mid_fwd: [0.0010000000474974513, 0.001500000013038516, 0.0020000000949949026] + aux.ctl.2_voltage.deadband: 0.15000000596046448 + aux.ctl.2_voltage.rev_mid_fwd: [0.0, 1.649999976158142, 3.299999952316284] + aux.ctl.dead0: false + aux.ctl.mode: '' + aux.ctl.type: 0 + aux.power_output: false + aux.pull: 1 + drive.current_brake_pu: 0.20000000298023224 + drive.flux_weakening.bemf_pid: [4.0, 2.0, 0.009999999776482582] + drive.flux_weakening.voltage_boost: 1.0 + drive.observer.0_ekf.proc_noise: [1000000.0, 3000000.0, 100000000.0] + drive.observer.0_ekf.sched_factor: [0.800000011920929, 0.949999988079071] + drive.observer.1_mras.gain: 1000.0 + drive.observer.type: 0 + drive.pre_calibrate: false + drive.runner.0_ramp.spinup_current_pu: [0.5, 0.5] + drive.runner.0_ramp.spinup_duration: [0.20000000298023224, 0.4000000059604645, 1.5, 0.0020000000949949026, 0.014999999664723873, 0.6000000238418579] + drive.runner.0_ramp.velocity_stall_spinup: [5.0, 20.0] + drive.runner.1_passive.delay: 0.20000000298023224 + drive.runner.1_passive.velocity_off_on: [10.0, 20.0] + drive.runner.type: 0 + drive.stall_limit: 20 + drive.velocity_ctl.1_pid.feedforward: [0.0, 0.0] + drive.velocity_ctl.1_pid.gain: [0.0, 0.0, 0.0] + drive.velocity_ctl.2_indi.acceleration_pi: [50.0, 0.0] + drive.velocity_ctl.2_indi.mass: 9.999999747378752e-06 + drive.velocity_ctl.acceleration: [-2000.0, 3000.0] + drive.velocity_ctl.type: 2 + drive.voltage_clipping: false + mns.deadman_timeout: 0.5 + mns.local_timestamp: false + mns.pub_interval_min: 0.05000000074505806 + mns.ratiometric_setpoint_min: 0.0 + mns.ratiometric_to_absolute_mul: 0.0 + mns.setpoint_index: 0 + mns.status_period: 1.0 + motor.current_ctl_bwr: 0.05000000074505806 + motor.current_max: 59.0 + motor.current_ramp: 5000.0 + motor.flux_linkage: 0.0007803703774698079 + motor.inductance_dq: [1.6388102039854857e-06, 1.6388102039854857e-06] + motor.mechanical_ratio: 12 + motor.resistance: 0.01189023070037365 + motor.thermistor_v2k: [0.0, 0.0, 0.0] + motor.voltage_ramp: 20.0 + servo.profile.acceleration: [-200.0, 100.0] + servo.profile.jerk: 2000.0 + servo.profile.velocity: [-50.0, 50.0] + standby.brake_voltage: .nan + sys.debug: false + sys.golden: sys. vsi. + uavcan.can.bitrate: [1000000, 1000000] + uavcan.can.count: 1 + uavcan.node.description: '' + uavcan.node.id: 125 + uavcan.pub.compact.id: 65535 + uavcan.pub.compact.prio: 3 + uavcan.pub.dq.id: 65535 + uavcan.pub.dq.prio: 3 + uavcan.pub.dynamics.id: 100 + uavcan.pub.dynamics.prio: 3 + uavcan.pub.feedback.id: 102 + uavcan.pub.feedback.prio: 3 + uavcan.pub.power.id: 101 + uavcan.pub.power.prio: 3 + uavcan.pub.status.id: 65535 + uavcan.pub.status.prio: 4 + uavcan.pub.temperature.id: 65535 + uavcan.pub.temperature.prio: 5 + uavcan.srv.low_level_io.id: 65535 + uavcan.sub.readiness.id: 65535 + uavcan.sub.setpoint_dynamics.id: 65535 + uavcan.sub.setpoint_rat_torque.id: 111 + uavcan.sub.setpoint_rat_torque_u9.id: 110 + uavcan.sub.setpoint_rat_velocity_u9.id: 65535 + uavcan.sub.setpoint_rat_voltage.id: 112 + uavcan.sub.setpoint_rat_voltage_u9.id: 65535 + uavcan.sub.setpoint_servo.id: 65535 + uavcan.sub.setpoint_velocity.id: 115 + vsi.activation_latency: 0.009999999776482582 + vsi.bridge_resistance: [0.0020000000949949026, 0.003000000026077032, 0.0020000000949949026, 0.003000000026077032, 0.0020000000949949026, 0.0020000000949949026] + vsi.calibration_duration: 2.0 + vsi.dc_voltage_gain: 18.799999237060547 + vsi.hw_fault_latency: 0.10000000149011612 + vsi.phase_current_gain: [100.0, 100.0, 25.0, 25.0] + vsi.phase_current_gain_attack_decay: [1.5, 0.36000001430511475] + vsi.phase_current_gain_decay_time: 0.10000000149011612 + vsi.phase_current_sampling_window: 1.9000000293090125e-06 + vsi.phase_current_stderr: [0.30000001192092896, 0.30000001192092896, 0.20000000298023224, 0.20000000298023224] + vsi.pwm_dead_time: 0.0 + vsi.pwm_freq_mul_log2: 0 + vsi.shortest_time_in_disabled_state: 1.9999999494757503e-05 + vsi.swap_ab: false + vsi.thermistor_v2k: [223.14999389648438, 100.0, 0.0] + vsi.tick_freq: 29402.15625 + zubax.cookie: '' +126: + aux.ctl.1_rcpwm.deadband: 9.999999747378752e-05 + aux.ctl.1_rcpwm.rev_mid_fwd: [0.0010000000474974513, 0.001500000013038516, 0.0020000000949949026] + aux.ctl.2_voltage.deadband: 0.15000000596046448 + aux.ctl.2_voltage.rev_mid_fwd: [0.0, 1.649999976158142, 3.299999952316284] + aux.ctl.dead0: false + aux.ctl.mode: '' + aux.ctl.type: 0 + aux.power_output: false + aux.pull: 1 + drive.current_brake_pu: 0.20000000298023224 + drive.flux_weakening.bemf_pid: [4.0, 2.0, 0.009999999776482582] + drive.flux_weakening.voltage_boost: 1.0 + drive.observer.0_ekf.proc_noise: [1000000.0, 3000000.0, 100000000.0] + drive.observer.0_ekf.sched_factor: [0.800000011920929, 0.949999988079071] + drive.observer.1_mras.gain: 1000.0 + drive.observer.type: 0 + drive.pre_calibrate: false + drive.runner.0_ramp.spinup_current_pu: [0.5, 0.5] + drive.runner.0_ramp.spinup_duration: [0.20000000298023224, 0.4000000059604645, 1.5, 0.0020000000949949026, 0.014999999664723873, 0.6000000238418579] + drive.runner.0_ramp.velocity_stall_spinup: [5.0, 20.0] + drive.runner.1_passive.delay: 0.20000000298023224 + drive.runner.1_passive.velocity_off_on: [10.0, 20.0] + drive.runner.type: 0 + drive.stall_limit: 20 + drive.velocity_ctl.1_pid.feedforward: [0.0, 0.0] + drive.velocity_ctl.1_pid.gain: [0.0, 0.0, 0.0] + drive.velocity_ctl.2_indi.acceleration_pi: [50.0, 0.0] + drive.velocity_ctl.2_indi.mass: 9.999999747378752e-06 + drive.velocity_ctl.acceleration: [-2000.0, 3000.0] + drive.velocity_ctl.type: 2 + drive.voltage_clipping: false + mns.deadman_timeout: 0.5 + mns.local_timestamp: false + mns.pub_interval_min: 0.05000000074505806 + mns.ratiometric_setpoint_min: 0.0 + mns.ratiometric_to_absolute_mul: 0.0 + mns.setpoint_index: 0 + mns.status_period: 1.0 + motor.current_ctl_bwr: 0.05000000074505806 + motor.current_max: 59.0 + motor.current_ramp: 5000.0 + motor.flux_linkage: 0.0008214705158025026 + motor.inductance_dq: [1.6596793557255296e-06, 1.6596793557255296e-06] + motor.mechanical_ratio: 12 + motor.resistance: 0.01172245666384697 + motor.thermistor_v2k: [0.0, 0.0, 0.0] + motor.voltage_ramp: 20.0 + servo.profile.acceleration: [-200.0, 100.0] + servo.profile.jerk: 2000.0 + servo.profile.velocity: [-50.0, 50.0] + standby.brake_voltage: .nan + sys.debug: false + sys.golden: sys. vsi. + uavcan.can.bitrate: [1000000, 1000000] + uavcan.can.count: 1 + uavcan.node.description: '' + uavcan.node.id: 126 + uavcan.pub.compact.id: 65535 + uavcan.pub.compact.prio: 3 + uavcan.pub.dq.id: 65535 + uavcan.pub.dq.prio: 3 + uavcan.pub.dynamics.id: 100 + uavcan.pub.dynamics.prio: 3 + uavcan.pub.feedback.id: 102 + uavcan.pub.feedback.prio: 3 + uavcan.pub.power.id: 101 + uavcan.pub.power.prio: 3 + uavcan.pub.status.id: 65535 + uavcan.pub.status.prio: 4 + uavcan.pub.temperature.id: 65535 + uavcan.pub.temperature.prio: 5 + uavcan.srv.low_level_io.id: 65535 + uavcan.sub.readiness.id: 65535 + uavcan.sub.setpoint_dynamics.id: 65535 + uavcan.sub.setpoint_rat_torque.id: 111 + uavcan.sub.setpoint_rat_torque_u9.id: 110 + uavcan.sub.setpoint_rat_velocity_u9.id: 65535 + uavcan.sub.setpoint_rat_voltage.id: 112 + uavcan.sub.setpoint_rat_voltage_u9.id: 65535 + uavcan.sub.setpoint_servo.id: 65535 + uavcan.sub.setpoint_velocity.id: 116 + vsi.activation_latency: 0.009999999776482582 + vsi.bridge_resistance: [0.0020000000949949026, 0.003000000026077032, 0.0020000000949949026, 0.003000000026077032, 0.0020000000949949026, 0.0020000000949949026] + vsi.calibration_duration: 2.0 + vsi.dc_voltage_gain: 18.799999237060547 + vsi.hw_fault_latency: 0.10000000149011612 + vsi.phase_current_gain: [100.0, 100.0, 25.0, 25.0] + vsi.phase_current_gain_attack_decay: [1.5, 0.36000001430511475] + vsi.phase_current_gain_decay_time: 0.10000000149011612 + vsi.phase_current_sampling_window: 1.9000000293090125e-06 + vsi.phase_current_stderr: [0.30000001192092896, 0.30000001192092896, 0.20000000298023224, 0.20000000298023224] + vsi.pwm_dead_time: 0.0 + vsi.pwm_freq_mul_log2: 0 + vsi.shortest_time_in_disabled_state: 1.9999999494757503e-05 + vsi.swap_ab: false + vsi.thermistor_v2k: [223.14999389648438, 100.0, 0.0] + vsi.tick_freq: 29402.15625 + zubax.cookie: '' diff --git a/setup_yakut_slcan.sh b/setup_yakut_slcan.sh index dc98311..9fcbb39 100755 --- a/setup_yakut_slcan.sh +++ b/setup_yakut_slcan.sh @@ -10,7 +10,7 @@ else fi if ! [ -e /sys/class/net/can0 ]; then - sudo ./setup_slcan.sh --remove-all --basename can --speed-code 5 /dev/serial/by-id/usb-Zubax*Babel* + sudo ./setup_slcan.sh --remove-all --basename can --speed-code 8 /dev/serial/by-id/usb-Zubax*Babel* fi export UAVCAN__CAN__IFACE='socketcan:can0' diff --git a/src/Node.cpp b/src/Node.cpp index 4ec3c16..e021c31 100644 --- a/src/Node.cpp +++ b/src/Node.cpp @@ -47,6 +47,7 @@ Node::Node() , _target_angular_velocity_x{0. * rad/s} , _target_angular_velocity_y{0. * rad/s} , _target_angular_velocity_z{0. * rad/s} +, _imu_qos_profile{rclcpp::KeepLast(10), rmw_qos_profile_sensor_data} { init_cyphal_heartbeat(); init_cyphal_node_info(); @@ -55,7 +56,7 @@ Node::Node() declare_parameter("can_node_id", 100); RCLCPP_INFO(get_logger(), - "configuring CAN bus:\n\tDevice: %s\n\tNode Id: %ld", + "configuring CAN2233 bus:\n\tDevice: %s\n\tNode Id: %ld", get_parameter("can_iface").as_string().c_str(), get_parameter("can_node_id").as_int()); @@ -78,11 +79,18 @@ Node::Node() }); init_teleop_sub(); + init_imu_sub(); _ctrl_loop_timer = create_wall_timer(CTRL_LOOP_RATE, [this]() { this->ctrl_loop(); }); _cyphal_demo_pub = _node_hdl.create_publisher(CYPHAL_DEMO_PORT_ID, 1*1000*1000UL); + _setpoint_velocity_pub_1 = _node_hdl.create_publisher(SETPOINT_VELOCITY_ID_1, 1*1000*1000UL); + _setpoint_velocity_pub_2 = _node_hdl.create_publisher(SETPOINT_VELOCITY_ID_2, 1*1000*1000UL); + _setpoint_velocity_pub_3 = _node_hdl.create_publisher(SETPOINT_VELOCITY_ID_3, 1*1000*1000UL); + _setpoint_velocity_pub_4 = _node_hdl.create_publisher(SETPOINT_VELOCITY_ID_4, 1*1000*1000UL); + + RCLCPP_INFO(get_logger(), "%s init complete.", get_name()); } @@ -114,6 +122,7 @@ void Node::init_cyphal_heartbeat() { std::lock_guard lock(_node_mtx); _cyphal_heartbeat_pub->publish(msg); + } }); } @@ -214,15 +223,140 @@ void Node::init_teleop_sub() _teleop_sub_options); } +void Node::init_imu_sub() +{ + declare_parameter("imu_topic", "imu"); + declare_parameter("imu_topic_deadline_ms", 100); + declare_parameter("imu_topic_liveliness_lease_duration", 1000); + + auto const imu_topic = get_parameter("imu_topic").as_string(); + auto const imu_topic_deadline = std::chrono::milliseconds(get_parameter("imu_topic_deadline_ms").as_int()); + auto const imu_topic_liveliness_lease_duration = std::chrono::milliseconds(get_parameter("imu_topic_liveliness_lease_duration").as_int()); + + _imu_qos_profile.deadline(imu_topic_deadline); + _imu_qos_profile.liveliness(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC); + _imu_qos_profile.liveliness_lease_duration(imu_topic_liveliness_lease_duration); + + _imu_sub_options.event_callbacks.deadline_callback = + [this, imu_topic](rclcpp::QOSDeadlineRequestedInfo & event) -> void + { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5*1000UL, + "deadline missed for \"%s\" (total_count: %d, total_count_change: %d).", + imu_topic.c_str(), event.total_count, event.total_count_change); + }; + + _imu_sub_options.event_callbacks.liveliness_callback = + [this, imu_topic](rclcpp::QOSLivelinessChangedInfo & event) -> void + { + if (event.alive_count > 0) + { + RCLCPP_INFO(get_logger(), "liveliness gained for \"%s\"", imu_topic.c_str()); + } + else + { + RCLCPP_WARN(get_logger(), "liveliness lost for \"%s\"", imu_topic.c_str()); + } + }; + + _imu_sub = create_subscription( + imu_topic, + _imu_qos_profile, + [this](sensor_msgs::msg::Imu::SharedPtr const msg) + { + _imu_data = *msg; + + RCLCPP_INFO(get_logger(), + "IMU Pose (x,y,z,w): %0.2f %0.2f %0.2f %0.2f", + _imu_data.orientation.x, + _imu_data.orientation.y, + _imu_data.orientation.z, + _imu_data.orientation.w); + }, + _imu_sub_options); +} + void Node::ctrl_loop() { - /* TODO: implement me ... */ + // Check if IMU data is available + if (!_imu_data.orientation_covariance.empty()) + { + // Log the x orientation data + RCLCPP_INFO(get_logger(), "IMU x orientation: %f", _imu_data.orientation.x); + + // Check if x orientation data exceeds 0.2 + if (_imu_data.orientation.x > 0.2) + { + // Call motor + zubax::primitive::real16::Vector4_1_0 const motor_msg{150.0, 100.0, 100.0, 10.0}; + _setpoint_velocity_pub_1->publish(motor_msg); + } + else + { + // Do something else if x orientation data does not exceed 0.2 + // For example, stop the motor + zubax::primitive::real16::Vector4_1_0 const motor_msg{10.0, 100.0, 10.0, 10.0}; + _setpoint_velocity_pub_1->publish(motor_msg); + } + + if (_imu_data.orientation.y > 0.2) + { + // Call motor + zubax::primitive::real16::Vector4_1_0 const motor_msg{150.0, 100.0, 100.0, 10.0}; + _setpoint_velocity_pub_4->publish(motor_msg); + } + else + { + // Do something else if x orientation data does not exceed 0.2 + // For example, stop the motor + zubax::primitive::real16::Vector4_1_0 const motor_msg{10.0, 100.0, 10.0, 10.0}; + _setpoint_velocity_pub_4->publish(motor_msg); + } + + if (_imu_data.orientation.z < 0.6) + { + // Call motor + zubax::primitive::real16::Vector4_1_0 const motor_msg{150.0, 100.0, 100.0, 10.0}; + _setpoint_velocity_pub_2->publish(motor_msg); + } + else + { + // Do something else if x orientation data does not exceed 0.2 + // For example, stop the motor + zubax::primitive::real16::Vector4_1_0 const motor_msg{10.0, 100.0, 10.0, 10.0}; + _setpoint_velocity_pub_2->publish(motor_msg); + + if (_imu_data.orientation.w < 0.65) + { + // Call motor + zubax::primitive::real16::Vector4_1_0 const motor_msg{150.0, 100.0, 100.0, 10.0}; + _setpoint_velocity_pub_3->publish(motor_msg); + } + else + { + // Do something else if x orientation data does not exceed 0.2 + // For example, stop the motor + zubax::primitive::real16::Vector4_1_0 const motor_msg{10.0, 100.0, 10.0, 10.0}; + _setpoint_velocity_pub_3->publish(motor_msg); + } + + + } + } + else + { + RCLCPP_WARN(get_logger(), "No IMU data available."); + } + + // Publish demo message static int8_t demo_cnt = 0; uavcan::primitive::scalar::Integer8_1_0 const demo_msg{demo_cnt}; _cyphal_demo_pub->publish(demo_msg); + + // Increment demo counter demo_cnt++; } + /************************************************************************************** * NAMESPACE **************************************************************************************/