search

ezWheelSAS / swd_ros_controllers

ROS nodes to control motors powered by the ez-Wheel Safety Wheel Drive (SWD®) technology.
https://www.ez-wheel.com/
GNU Lesser General Public License v2.1
5 stars 4 forks source link

Left monitor is not running #29

Closed suguruhamada1003910 closed 2 years ago

suguruhamada1003910 commented 2 years ago

After commissioning with swd_left_4_commissioning.py , status LED of left motor is flasing with red color and left motor is not running. When I do "starter_kit.launch", "Inconsistant STO for left and right motors, left=0, right=1." is displayed. How should I do to resolve this error?

swd_sk@C-Latitude-7280:~$ roslaunch swd_starter_kit_bringup starter_kit.launch 
... logging to /home/swd_sk/.ros/log/7dd7b90e-0e0e-11ed-8a82-3f8c84e5ebb9/roslaunch-C-Latitude-7280-2840.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://C-Latitude-7280:34541/

SUMMARY
========

PARAMETERS
 * /hector_mapping/advertise_map_service: True
 * /hector_mapping/base_frame: base_link
 * /hector_mapping/laser_z_max_value: 1.0
 * /hector_mapping/laser_z_min_value: -1.0
 * /hector_mapping/map_frame: map
 * /hector_mapping/map_multi_res_levels: 2
 * /hector_mapping/map_resolution: 0.05
 * /hector_mapping/map_size: 2048
 * /hector_mapping/map_start_x: 0.5
 * /hector_mapping/map_start_y: 0.5
 * /hector_mapping/map_update_angle_thresh: 0.06
 * /hector_mapping/map_update_distance_thresh: 0.4
 * /hector_mapping/odom_frame: odom
 * /hector_mapping/pub_map_odom_transform: True
 * /hector_mapping/scan_subscriber_queue_size: 5
 * /hector_mapping/scan_topic: /laser_1/scan
 * /hector_mapping/tf_map_scanmatch_transform_frame_name: scanmatcher_frame
 * /hector_mapping/update_factor_free: 0.4
 * /hector_mapping/update_factor_occupied: 0.9
 * /hector_mapping/use_tf_pose_start_estimate: False
 * /hector_mapping/use_tf_scan_transformation: True
 * /rosbridge_websocket/address: 10.0.0.4
 * /rosbridge_websocket/use_compression: True
 * /rosdistro: noetic
 * /rosversion: 1.15.14
 * /swd_diff_drive_controller/base_frame: base_link
 * /swd_diff_drive_controller/baseline_m: 0.485
 * /swd_diff_drive_controller/command_timeout_ms: 500
 * /swd_diff_drive_controller/control_mode: Twist
 * /swd_diff_drive_controller/have_backward_sls: False
 * /swd_diff_drive_controller/left_swd_config_file: /opt/ezw/usr/etc/...
 * /swd_diff_drive_controller/odom_frame: odom
 * /swd_diff_drive_controller/positive_polarity_wheel: Right
 * /swd_diff_drive_controller/pub_freq_hz: 50
 * /swd_diff_drive_controller/publish_odom: True
 * /swd_diff_drive_controller/publish_safety_functions: True
 * /swd_diff_drive_controller/publish_tf: True
 * /swd_diff_drive_controller/right_swd_config_file: /opt/ezw/usr/etc/...
 * /swd_diff_drive_controller/wheel_max_speed_rpm: 95
 * /swd_diff_drive_controller/wheel_safety_limited_speed_rpm: 40
 * /teleop_twist_joy/axis_angular: 3
 * /teleop_twist_joy/axis_linear: 1
 * /teleop_twist_joy/enable_button: 5
 * /teleop_twist_joy/enable_turbo_button: 6
 * /teleop_twist_joy/joy_config: xbox
 * /teleop_twist_joy/scale_angular: 0.7
 * /teleop_twist_joy/scale_angular_turbo: 1.4
 * /teleop_twist_joy/scale_linear: 1.0
 * /teleop_twist_joy/scale_linear_turbo: 1.3
 * /urg_node/frame_id: laser_1
 * /urg_node/ip_address: 10.0.0.5
 * /xbox_joystick/autorepeat_rate: 50

NODES
  /
    hector_mapping (hector_mapping/hector_mapping)
    laser_broadcaster (tf/static_transform_publisher)
    map_odom_broadcaster (tf/static_transform_publisher)
    robot_manager (swd_robot_manager/robot_manager)
    rosbridge_websocket (rosbridge_server/rosbridge_websocket)
    swd_diff_drive_controller (swd_ros_controllers/swd_diff_drive_controller)
    teleop_twist_joy (teleop_twist_joy/teleop_node)
    tf2_web_republisher (tf2_web_republisher/tf2_web_republisher)
    urg_node (urg_node/urg_node)
    xbox_joystick (joy/joy_node)

ROS_MASTER_URI=http://localhost:11311

process[swd_diff_drive_controller-1]: started with pid [2854]
process[urg_node-2]: started with pid [2855]
process[laser_broadcaster-3]: started with pid [2856]
[ INFO] [1658969468.819058918]: Ready !
process[hector_mapping-4]: started with pid [2862]
[ INFO] [1658969468.826691877]: Initializing swd_diff_drive_controller, node name : /swd_diff_drive_controller
process[map_odom_broadcaster-5]: started with pid [2869]
process[rosbridge_websocket-6]: started with pid [2874]
process[tf2_web_republisher-7]: started with pid [2879]
process[xbox_joystick-8]: started with pid [2881]
process[teleop_twist_joy-9]: started with pid [2883]
process[robot_manager-10]: started with pid [2889]
[ INFO] [1658969468.928254936]: Motors config files, right : /opt/ezw/usr/etc/ezw-smc-core/swd_right_config.ini, left : /opt/ezw/usr/etc/ezw-smc-core/swd_left_config.ini
[default] contextId = 12
[default] nodeId = 5
[default] canDevice = can0
[default] HWConfigurationEntry = SWD_RIGHT
[default] HWConfigurationFile = /opt/ezw/data/configuration.json
[default] SWCompatibilityFile = /opt/ezw/firmwares/compatibility.json
[default] CANOpenEDSFile = /opt/ezw/usr/etc/ezw-canopen-dico/swd_core.eds
[default] targetVelocityTimeoutMs = 300
[default] maxMaintenanceSpeedValue = 400
[default] contextId = 12
[default] nodeId = 5
[default] canDevice = can0
[default] HWConfigurationEntry = SWD_RIGHT
[default] HWConfigurationFile = /opt/ezw/data/configuration.json
[default] SWCompatibilityFile = /opt/ezw/firmwares/compatibility.json
[default] CANOpenEDSFile = /opt/ezw/usr/etc/ezw-canopen-dico/swd_core.eds
[default] targetVelocityTimeoutMs = 300
[default] maxMaintenanceSpeedValue = 400
2022-7-28 9:51:8;;SMCDS;INFO;getHWConfigurationMember;90;HardwareHelper;Search for entry SWD_RIGHT / nbStepRevolutionElectype Integer into /opt/ezw/data/configuration.json;
2022-7-28 9:51:8;;SMCDS;INFO;getHWConfigurationMember;111;HardwareHelper;nbStepRevolutionElec : 6;
2022-7-28 9:51:8;;SMCDS;INFO;getHWConfigurationMember;90;HardwareHelper;Search for entry SWD_RIGHT / nbPolePairtype Integer into /opt/ezw/data/configuration.json;
2022-7-28 9:51:8;;SMCDS;INFO;getHWConfigurationMember;111;HardwareHelper;nbPolePair : 5;
2022-7-28 9:51:8;;SMCDS;INFO;getHWConfigurationMember;145;HardwareHelper;Search for entry SWD_RIGHT / reduction type Double into /opt/ezw/data/configuration.json;
2022-7-28 9:51:8;;SMCDS;INFO;getHWConfigurationMember;166;HardwareHelper;reduction : 14.000000;
2022-7-28 9:51:8;;SMCDS;INFO;getHWConfigurationMember;145;HardwareHelper;Search for entry SWD_RIGHT / diameter type Double into /opt/ezw/data/configuration.json;
2022-7-28 9:51:8;;SMCDS;INFO;getHWConfigurationMember;166;HardwareHelper;diameter : 125.000000;
2022-7-28 9:51:8;;SMCDS;INFO;getHWConfigurationMember;200;HardwareHelper;Search for entry SWD_RIGHT / device type String into /opt/ezw/data/configuration.json;
2022-7-28 9:51:8;;SMCDS;INFO;getHWConfigurationMember;221;HardwareHelper;device : /dev/ttyUSBY;
2022-7-28 9:51:8;;APP;INFO;init;76;DBusClientDelegate;Checking DBUS proxy availability : commonapi.ezw.canopenservice;
HectorSM map lvl 0: cellLength: 0.05 res x:2048 res y: 2048
[ WARN] [1658969469.043012464]: Couldn't set gain on joystick force feedback: Bad file descriptor
2022-7-28 9:51:9;;APP;INFO;init;89;DBusClientDelegate;DBUS Proxy available : commonapi.ezw.canopenservice;
HectorSM map lvl 1: cellLength: 0.1 res x:1024 res y: 1024
[ INFO] [1658969469.124689695]: HectorSM p_base_frame_: base_link
[ INFO] [1658969469.126477387]: HectorSM p_map_frame_: map
[ INFO] [1658969469.126534284]: HectorSM p_odom_frame_: odom
[ INFO] [1658969469.126564546]: HectorSM p_scan_topic_: /laser_1/scan
[ INFO] [1658969469.126600367]: HectorSM p_use_tf_scan_transformation_: true
[ INFO] [1658969469.126633996]: HectorSM p_pub_map_odom_transform_: true
[ INFO] [1658969469.126665387]: HectorSM p_scan_subscriber_queue_size_: 5
[ INFO] [1658969469.126698830]: HectorSM p_map_pub_period_: 2.000000
[ INFO] [1658969469.126733710]: HectorSM p_update_factor_free_: 0.400000
[ INFO] [1658969469.126803409]: HectorSM p_update_factor_occupied_: 0.900000
[ INFO] [1658969469.126844309]: HectorSM p_map_update_distance_threshold_: 0.400000 
[ INFO] [1658969469.126877122]: HectorSM p_map_update_angle_threshold_: 0.060000
[ INFO] [1658969469.126917401]: HectorSM p_laser_z_min_value_: -1.000000
[ INFO] [1658969469.126950107]: HectorSM p_laser_z_max_value_: 1.000000
2022-7-28 9:51:9;;SMCDS;INFO;init;83;Init Success;
[default] contextId = 12
[default] nodeId = 4
[default] canDevice = can0
[default] HWConfigurationEntry = SWD_LEFT
[default] HWConfigurationFile = /opt/ezw/data/configuration.json
[default] SWCompatibilityFile = /opt/ezw/firmwares/compatibility.json
[default] CANOpenEDSFile = /opt/ezw/usr/etc/ezw-canopen-dico/swd_core.eds
[default] targetVelocityTimeoutMs = 300
[default] maxMaintenanceSpeedValue = 400
[default] contextId = 12
[default] nodeId = 4
[default] canDevice = can0
[default] HWConfigurationEntry = SWD_LEFT
[default] HWConfigurationFile = /opt/ezw/data/configuration.json
[default] SWCompatibilityFile = /opt/ezw/firmwares/compatibility.json
[default] CANOpenEDSFile = /opt/ezw/usr/etc/ezw-canopen-dico/swd_core.eds
[default] targetVelocityTimeoutMs = 300
[default] maxMaintenanceSpeedValue = 400
2022-7-28 9:51:9;;SMCDS;INFO;getHWConfigurationMember;90;HardwareHelper;Search for entry SWD_LEFT / nbStepRevolutionElectype Integer into /opt/ezw/data/configuration.json;
2022-7-28 9:51:9;;SMCDS;INFO;getHWConfigurationMember;111;HardwareHelper;nbStepRevolutionElec : 6;
2022-7-28 9:51:9;;SMCDS;INFO;getHWConfigurationMember;90;HardwareHelper;Search for entry SWD_LEFT / nbPolePairtype Integer into /opt/ezw/data/configuration.json;
2022-7-28 9:51:9;;SMCDS;INFO;getHWConfigurationMember;111;HardwareHelper;nbPolePair : 5;
2022-7-28 9:51:9;;SMCDS;INFO;getHWConfigurationMember;145;HardwareHelper;Search for entry SWD_LEFT / reduction type Double into /opt/ezw/data/configuration.json;
2022-7-28 9:51:9;;SMCDS;INFO;getHWConfigurationMember;166;HardwareHelper;reduction : 14.000000;
2022-7-28 9:51:9;;SMCDS;INFO;getHWConfigurationMember;145;HardwareHelper;Search for entry SWD_LEFT / diameter type Double into /opt/ezw/data/configuration.json;
2022-7-28 9:51:9;;SMCDS;INFO;getHWConfigurationMember;166;HardwareHelper;diameter : 125.000000;
2022-7-28 9:51:9;;SMCDS;INFO;getHWConfigurationMember;200;HardwareHelper;Search for entry SWD_LEFT / device type String into /opt/ezw/data/configuration.json;
2022-7-28 9:51:9;;SMCDS;INFO;getHWConfigurationMember;221;HardwareHelper;device : /dev/ttyUSBX;
2022-7-28 9:51:9;;APP;INFO;init;76;DBusClientDelegate;Checking DBUS proxy availability : commonapi.ezw.canopenservice;
2022-7-28 9:51:9;;APP;INFO;init;89;DBusClientDelegate;DBUS Proxy available : commonapi.ezw.canopenservice;
2022-7-28 9:51:9;;SMCDS;INFO;init;83;Init Success;
[ INFO] [1658969469.441499884]: Got parameter 'wheel_max_speed_rpm' = 95.000000 rpm. Setting maximum motor speed to 1330 rpm
[ INFO] [1658969469.441548834]: Got parameter 'wheel_safety_limited_speed_rpm' = 40.000000 rpm. Setting maximum motor safety limited speed to 560 rpm
[ INFO] [1658969469.441687189]: ez-Wheel's swd_diff_drive_controller initialized successfully!
[ INFO] [1658969469.441758564]: front=1 count=1
[ INFO] [1658969469.441791766]: Got Twist command: linear = 0.000000 m/s, angular = 0.000000 rad/s. Calculated speeds (left, right) = (2139, 2139) rpm
[ WARN] [1658969469.441817926]: The target speed exceeds the maximum speed limit (1330 rpm). Speed set to (left, right) (1330, 1330) rpm
[ INFO] [1658969469.442886335]: Speed sent to motors (left, right) = (1330, 1330) rpm
[ERROR] [1658969469.684125267]: Inconsistant STO for left and right motors, left=0, right=1.
[ INFO] [1658969469.739378414]: STO: 0, SDI+: 1, SLS: 1
[ERROR] [1658969469.880032505]: Inconsistant STO for left and right motors, left=0, right=1.
[ INFO] [1658969469.921634121]: STO: 0, SDI+: 1, SLS: 1
[ERROR] [1658969470.083034339]: Inconsistant STO for left and right motors, left=0, right=1.
[ INFO] [1658969470.138329517]: STO: 0, SDI+: 1, SLS: 1
[ERROR] [1658969470.279273162]: Inconsistant STO for left and right motors, left=0, right=1.
[ INFO] [1658969470.328824868]: STO: 0, SDI+: 1, SLS: 1
2022-7-28 9:51:10;;SMCDS;INFO;enterInOperationEnabledState;515;Controller;Check NOT_READY_TO_SWITCH_ON;
2022-7-28 9:51:10;;SMCDS;INFO;enterInOperationEnabledState;535;Controller;Check FAULT;
2022-7-28 9:51:10;;SMCDS;INFO;enterInOperationEnabledState;544;Controller;FAULT detected;
^C2022-7-28 9:51:10;;SMCDS;INFO;enterInOperationEnabledState;562;Controller;Check SWITCH_ON_DISABLED;
2022-7-28 9:51:10;;SMCDS;INFO;enterInOperationEnabledState;582;Controller;Check READY_TO_SWITCH_ON;
2022-7-28 9:51:10;;SMCDS;INFO;enterInOperationEnabledState;603;Controller;Check SWITCHED_ON;
2022-7-28 9:51:10;;SMCDS;INFO;enterInOperationEnabledState;623;Controller;Check OPERATION_ENABLED;
2022-7-28 9:51:10;;SMCDS;ERROR;enterInOperationEnabledState;632;Controller;Not in OPERATION_ENABLED;
[ERROR] [1658969470.755389176]: Failed to set PDS state for left motor, EZW_ERR: SMCService : Controller::enterInOperationEnabledState() return error code : 6
[ WARN] [1658969470.755432905]: PDS state machine is not OK.
suguruhamada1003910 commented 2 years ago

It was a wiring mistake. STO means emergency switch in starter kit.