Closed millejade closed 2 months ago
For additional info:
This is my bus.yml
:
# General options especially dcf_path
options:
dcf_path: "@BUS_CONFIG_PATH@"
# The configuration of the master
master:
node_id: 2
driver: "ros2_canopen::MasterDriver"
package: "canopen_master_driver"
sync_period: 20000 # The SYNC interval in μs (default: 0)
# Defaults that apply to all slave nodes
defaults:
driver: "ros2_canopen::Cia402Driver"
package: "canopen_402_driver"
polling: true
# Configurations for all slave nodes
nodes:
dev:
dcf: "dev.dcf"
node_id: 1
And this is my launch file:
dev_canopen.launch.py
def generate_launch_description():
ld = LaunchDescription()
device_container = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[
os.path.join(get_package_share_directory("canopen_core"), "launch"),
"/canopen.launch.py",
]
),
launch_arguments={
"master_config": os.path.join(
get_package_share_directory("dev_canopen"),
"config",
"cia402",
"master.dcf",
),
"master_bin": os.path.join(
get_package_share_directory("dev_canopen"),
"config",
"cia402",
"master.bin",
),
"bus_config": os.path.join(
get_package_share_directory("dev_canopen"),
"config",
"cia402",
"bus.yml",
),
"can_interface_name": "can0",
}.items(),
)
ld.add_action(device_container)
return ld
Hi, I haven't seen this error yet. It seems, that 60ff is not mapped as PDO even though it is indicated as mapped in your dcf.
You can check how to do pdo configuration in bus.yaml here canopen_tests/config/cia402/bus.yml. For Velocity mode at least 606c (actual velocity) as tpdo an 60ff (target velocity) as rpdo are necessary.
Hello,
Thanks for quick response!
That means I'm doing the correct process of sending velocity?
/velocity_mode
through Service/target
to some values through Service to move the motor?(It was because I've seen in the documentation a subscriber /target
but it doesn't really appear when I'm doing ros2 topic list
.)
Hello, just an update!
It is indeed an issue on my TPDO4 and RPDO4 - they are disabled in the device so I had to enable it in a DCF/EDS file accordingly.
I enabled it and I do not encounter any error after doing this:
/velocity_mode
through Service/target
to some values through Service to move the motor
But this service does not move the motor at all.
/target
service returns trueThen I then tried to use /sdo_write
service:
ros2 service call /tmcm_1241/sdo_write canopen_interfaces/srv/COWrite "{index: 0x60FF, subindex: 0, data: 50000}"
This one works and the motor starts moving.
@hellantos, questions:
/target
service won't work? /target
but I don't see it when doing ros2 topic list
, why is that?Hello @hellantos !
Any insights on my previous questions?
Thank you!
@millejade Can you check if the pdos are sent by using candump or a similar logging tool? If you only enabled in the dcf but not on the device, this will make ros2_canopen believe it is enabled, while the device does not have the pdos enabled.
If you want ros2 canopen to enable the pdos please specify the pdos in bus.yml.
Thanks @hellantos!
Maybe the PDOs are enabled in the DCF but not in the device.
If you want ros2 canopen to enable the pdos please specify the pdos in bus.yml.
Do you mean, if the PDOs were not enabled in the device, I can enable it using bus.yml
? (Given that the PDOs are enabled in the DCF)
This has been solved. Thanks @hellantos!
The TPDO and RPDO is actually disabled. Even if I enabled it on the DCF/EDS and enabled it using the device's IDE, it still didn't work but enabling it only using sdo
in bus.yml
works. For me, I have to rely on the FW manual of the device where index 0x1403 is my RPDO4 and 0x1803 is my TPDO4 - these are the PDOs that I need for Velocity control. You can refer to the FW Manual of the device as I'm not sure if it is different for each device.
Bug I'm trying to use the velocity mode but I'm not sure if I'm doing it right. Position Mode is working well. If I'm doing it right, where I should focus to fix this issue on my end?
Logs This is the full logs on the
ros2 launch
terminalSetup:
Additional context So what I did was:
1. Do the roslaunch
This is my Launch Files:
These are the logs after launching it:
2. Call the Service
/init
After calling this service, I got this logs from the same terminal:
3. Call the Service to use Velocity Mode
I then called the service for velocity mode and it returns True, check the logs below:
4. Call the Service Target
This service call returns True - check the logs below:
On the other hand, this is the log from the ros2 launch terminal: