Implement parallelogram steering controller

[AbBaSaMo]

• [x] research parallelogram steering and the kinematics
• [x] look into how mechanical is going to implement parallelogram steering
• [ ] refer to https://github.com/Monash-Connected-Autonomous-Vehicle/ESDA/issues/7#issuecomment-1933484640

Refer to the docs here for writing a controller https://control.ros.org/humble/doc/ros2_controllers/doc/writing_new_controller.html Refer to the following section as a prompt on how to calculate odom using encoder data https://navigation.ros.org/setup_guides/odom/setup_odom.html#:~:text=An%20alternative%20to,in%20ros2_control.

[AbBaSaMo]

@Jokua @Jiawei-Liao

We are no longer using ros2_control so this issue has been updated. Your tasks are as follows:

• [x] create a new branch from main in our repo
• [x] create a ros package called esda_control with build type of ament_python
• [x] in the esda control package create a ros node in called paralellogram_steering_controller.py
• [x] the node should subscribe to topic /cmd_vel which publishes a twist message
• [ ] convert the linear velocity component to appropriate motor commands and publish these to a topic /target_motor_cmd
• [x] convert the angular z component to a servo position given the steering geometry and publish this servo position to a topic /target_servo_pos

The first 4 tasks should only take 10-15 minutes. The 5th task is relatively easy but will need you to confirm the min and max speeds for the motors as well as what unit of measurement you sent to the MCU chip @jaimasters The 6th task should build on top of the calculations and work @Jokua did so far

[Jiawei-Liao]

We started and completed most of this. Just need to confirm a few more things to finish.

Not sure on the MotorCommand and ServoPosition message types. Is it already created or do we need to make it ourselves. Also not sure on the contents in both.

Need to check with mechanical team (?) for limits of motor commands.

Are there any limits required for angular velocity? Also was not too sure what calculations are needed for the servo(s).

Pushed to ESDA repo on esda-control branch: https://github.com/Monash-Connected-Autonomous-Vehicle/ESDA/tree/esda-control/esda_control

[AbBaSaMo]

@Jiawei-Liao motor command is just the linear x component of the twist command you are reading from /cmd_vel so it's just a float. Confirmed this with Jai previously where the mcu does conversions to pwm and we only pass the linar velocity

For servoPosition, ask mechanical what the limits are for the servo i.e. the min angle and max angle it/the wheels or whatever can turn at and output that angle for now. Later on it'll have to be mapped but dw about the mapping, just output an int or float that is the angle.

Wdym limits for angular velocity? Did you mean limits for servo position?

[AbBaSaMo]

@Jiawei-Liao @Jokua also a reminder to use conventional commit messages e.g. prefix the message with the type of commit [feat, fix, refactro, docs] followed by a descriptive commit message

[AbBaSaMo]

https://github.com/ros-controls/ros_controllers/issues/412 might be useful

[AbBaSaMo]

@dylan-gonzalez @AnthonyZhOon any resources Jiawei and Will can use to help them in converting angular velocity to steering angle for Ackermanns geometry and given the use of paralellogram steering?

[AnthonyZhOon]

I believe a parallelogram structure results in parallel, not ackermann steering but here's a simple paper on calculating and comparing them. https://scholarsarchive.byu.edu/cgi/viewcontent.cgi?article=1063&context=studentpub_uht If it's parallel, it should be pretty simple as calculate the turning radius from linear vel. perpendicular to circle centre and angular velocity, then converting to target steering angle assuming 0 slip. r = twist.linear.x / twist.angular.z maybe that gets messy but MATLAB org produces resources for this kind of thing which you just plug numbers into, probably easiest to use and they check things https://www.mathworks.com/help/vdynblks/ref/kinematicsteering.html

[Jiawei-Liao]

Implemented dummy message contents, removed motor command limit check, implemented servo position publisher command

Questions we had: Are the message contents correct?

MotorCommand.msg: float64 left_wheel float64 right_wheel

ServoPosition.msg: float64 servo_position

Is the limit for wheel angle 46 in both directions or combined?

Assuming that for travelling in a straight line, servo position is 2000 (middle of 0 to 4000)

[AbBaSaMo]

@Jiawei-Liao @Jokua

Ok so I don't think I mentioned this explicitly but i'll lay it out in dot points below

parallelogram_steering_controller

• only takes relevant values from twist message and calculates the right steering angle
• outputs this steering angle to ros topic called /set_position so refer to the structure of the set_position topics msg in DynamixelSDK here https://github.com/ROBOTIS-GIT/DynamixelSDK/blob/humble-devel/dynamixel_sdk_custom_interfaces/msg/SetPosition.msg
• the ID is only 1 cause we will only have 1 servo
• the DynamixelSDK driver then reads this topic and turns the servo but you guys only worry about the top 2 parts

MCU_interface node

• this node reads the linear velocity so your controller doesn't need to worry about it
• this node then converts it to appropriate CAN message and sends it to the MCU chip as m/s
• confirming with Jai on if it's one speed value we send or 2. likely only 1

As for @Jokua 's quesiton on what I meant by limiting the 0-4000 range

• iirc the values we can send for servo position on the set_position topic are 0-4000
• we can only allow it to go 46 deg as shown in the image above so we can use 46/360 degrees [a ratio of 0.12777]
• if 0-4000 is 360 deg, we need to limit it to a range of 511 out of the entire 4000 [so 0.12777*4000]
• for example we only use positions 0-511 so the servo does not attempt to turn beyond 46 deg and break something
• the exact min and start position depends on how the servo is oriented by mechanical e.g. after testing the 511 range might have to be from 45-556

[AbBaSaMo]

@Jokua @Jiawei-Liao also don't re-implement the setposition msg, just add DynamixelSDK as a rosdep or alternatively clone the repo into your workspace if the latter results in any issues and build their package along side yours to have access to the message

you'll have to import it in your python node to use it

[Jiawei-Liao]

Fixed to include changes noted above. Just to double check, do we need to publish another message/topic for linear velocity for mcu chip or does mcu chip take linear velocity directly from /cmd_vel. Also need to find out length between front and back wheels.

After that I think just testing then its finished.

[AbBaSaMo]

No, dw about mcu chip or linear velocity. That will be done in another ROS node as part of another issue.

[AbBaSaMo]

@Jokua @Jiawei-Liao

Writing here for the sake of it being in one place. Now that the controller is done, do the following:

• [ ] test it while running the DynamixelSDK read_write_node
• [ ] create a launch file in esda_launch called control_launch.py that does the following
  • [x] launches the Dynamixel node
  • [x] launches the controller
• [ ] test the entire thing once more by running it using the newly created launch file this time

In the future we'll have to test the correctness of the controller by testing it in sim/hardware. @AnthonyZhOon any other tests or general things to take into consideration?

[AbBaSaMo]

@Jiawei-Liao @Jokua dw about testing the servo, there's some issues with the launch file and the servo connects when running the dynamixel sdk node but has some 'incorrect packet' error. For now just focus entirely on the MCU chip interface. The usb to can device is in a little box in the 2nd drawer where the other hardware is else it'll be connected to the hive.

[AbBaSaMo]

@Monash-Connected-Autonomous-Vehicle/avp-team

Getting the following error trying to run a launch file that was previously working

To reproduce, go to the esda repo, clone the esda-control branch build using the instructions in the main branches readme run the command shown in the screenshot

anyone know what's going on? found some ros threads that weren't too helpful. the recent commit isn't causing the issue as i get the same issue with the previous commit

[AbBaSaMo]

OK nm, it was a much earlier commit that was working and the issue was a trailing comma causing python to interpret a variable as a tuple.

[AbBaSaMo]

@Jokua @Jiawei-Liao i merged esda_control to main after some launch fixes and i'd like the branch to have the most recent changes on it b4 u continue working i.e. merge main into it but if you guys encounter issues or don't know what to do, then just delete the branch and create a new one from main.

[AbBaSaMo]

This is the issue I was referring to

Anyone from @Monash-Connected-Autonomous-Vehicle/avp-team know how to fix? Never encountered b4 from memory and the base example files used to always work.

For further context:

• this is the dynamixelSDK read_write_node running
• am sending a setPosition message to random positions and the message sends fine
• the issue is from the read_Write_node sending this over serial to the servo

Looked at some gh issues online, some suggest altering the baudrate, any other suggestions from anyone?

[AbBaSaMo]

Reduced the baudrate to 30kish and no change. Could something be broken?

[Jiawei-Liao]

I'm not sure what that message means either. I believe when @Jokua tested the launch file we made it worked? So maybe something went wrong when merging it with all the other nodes?

[AbBaSaMo]

nah, the issue persists with the base dynamixel sdk code so it's not the merge.

[AbBaSaMo]

Given launch and the node itself is working and a new servo is being bought, closing the issue. Testing the veracity of the implementation cannot be done until a sim/the build is set-up. A new issue will be created then.