# Integrate velocity control into ROS
**URL:** https://forum.hello-robot.com/t/integrate-velocity-control-into-ros/779
**Category:** Ask
**Created:** [July 31, 2023, 7:47pm UTC](https://forum.hello-robot.com/t/integrate-velocity-control-into-ros/779 "2023-07-31T19:47:44Z")
**Posts on this page:** 6
**Page:** 1
### Author:  [@CalaW](https://forum.hello-robot.com/u/CalaW)
#### Post date: [July 31, 2023, 7:47pm UTC](https://forum.hello-robot.com/t/integrate-velocity-control-into-ros/779/1 "2023-07-31T19:47:44Z")
I’m trying to do velocity control of the base, arm and wrist according to [this post](https://forum.hello-robot.com/t/velocity-control-using-ros/757). I also need the stretch\_driver in ROS to get the odometry information. However, they seem to conflict with each other. How can I get stretch\_ros to run with the stretch body sdk?
---
### Author:  [@CalaW](https://forum.hello-robot.com/u/CalaW)
#### Post date: [July 31, 2023, 7:51pm UTC](https://forum.hello-robot.com/t/integrate-velocity-control-into-ros/779/2 "2023-07-31T19:51:43Z")
I wrote a simple node to listen to the speed command in ros, and then operate the real robot. The node works perfectly when stretch\_driver is not running.
```python
#!/usr/bin/env python3
import rospy
import stretch_body.end_of_arm
import stretch_body.robot
import stretch_body.stepper
from std_msgs.msg import Float64, Float64MultiArray
class StretchController:
def __init__ (self) -> None:
self.r = stretch_body.robot.Robot()
self.r.startup()
assert self.r.is_calibrated() # the robot must be homed
rospy.Subscriber("/stretch_controller/joint_cmd", Float64MultiArray, self.joint_cb)
rospy.Subscriber("/stretch_controller/gripper_cmd", Float64, self.gripper_cb)
def joint_cb(self, msg: Float64MultiArray) -> None:
q_dot = msg.data
self.r.base.set_velocity(q_dot[0], q_dot[1])
self.r.lift.set_velocity(q_dot[2])
self.r.arm.set_velocity(sum(q_dot[3:7]))
self.r.end_of_arm.get_joint("wrist_yaw").set_velocity(q_dot[7])
self.r.push_command()
def gripper_cb(self, msg: Float64):
self.r.end_of_arm.get_joint("stretch_gripper").set_velocity(msg.data)
self.r.push_command()
if __name__ == " __main__":
rospy.init_node("stretch_controller")
ctrler = StretchController()
rospy.spin()
```
However when running this node with stretch\_driver, I got this error:
```plaintext
Traceback (most recent call last):
File "/usr/lib/python3/dist-packages/serial/serialposix.py", line 500, in read
raise SerialException(
serial.serialutil.SerialException: device reports readiness to read but returned no data (device disconnected or multiple access on port?)
During handling of the above exception, another exception occurred:
Traceback (most recent call last):
File "/home/hat/catkin_ws/src/stretch_ros/stretch_core/nodes/stretch_driver", line 541, in
node.main()
File "/home/hat/catkin_ws/src/stretch_ros/stretch_core/nodes/stretch_driver", line 413, in main
self.robot.startup()
File "/home/hat/stretch_body/body/stretch_body/robot.py", line 226, in startup
if not self.devices[k].startup(threaded=False):
File "/home/hat/stretch_body/body/stretch_body/head.py", line 24, in startup
return DynamixelXChain.startup(self, threaded=threaded)
File "/home/hat/stretch_body/body/stretch_body/dynamixel_X_chain.py", line 80, in startup
if not self.motors[mk].startup(threaded=False):
File "/home/hat/stretch_body/body/stretch_body/dynamixel_hello_XL430.py", line 183, in startup
if self.motor.do_ping(verbose=False):
File "/home/hat/stretch_body/body/stretch_body/dynamixel_XL430.py", line 292, in do_ping
dxl_model_number, dxl_comm_result, dxl_error = self.packet_handler.ping(self.port_handler, self.dxl_id)
File "/home/hat/.local/lib/python3.8/site-packages/dynamixel_sdk/protocol2_packet_handler.py", line 369, in ping
rxpacket, result, error = self.txRxPacket(port, txpacket)
File "/home/hat/.local/lib/python3.8/site-packages/dynamixel_sdk/protocol2_packet_handler.py", line 346, in txRxPacket
rxpacket, result = self.rxPacket(port)
File "/home/hat/.local/lib/python3.8/site-packages/dynamixel_sdk/protocol2_packet_handler.py", line 257, in rxPacket
rxpacket.extend(port.readPort(wait_length - rx_length))
File "/home/hat/.local/lib/python3.8/site-packages/dynamixel_sdk/port_handler.py", line 78, in readPort
return self.ser.read(length)
File "/usr/lib/python3/dist-packages/serial/serialposix.py", line 509, in read
raise SerialException('read failed: {}'.format(e))
serial.serialutil.SerialException: read failed: device reports readiness to read but returned no data (device disconnected or multiple access on port?)
```
---
### Author:  [@bshah](https://forum.hello-robot.com/u/bshah)
#### Post date: [July 31, 2023, 9:12pm UTC](https://forum.hello-robot.com/t/integrate-velocity-control-into-ros/779/3 "2023-07-31T21:12:22Z")
Hi @CalaW, stretch\_ros and stretch\_body cannot run at the same time. The reason is only one process can talk to the hardware at a time. This is why your node works when the stretch\_driver node is not running.
There’s two options available:
1. Some of what stretch\_driver does can also be done with stretch\_body. Depending on what functionality you need, you may be able to use stretch\_body solely. For example, if you are reading wheel odometry that doesn’t fuse lidar from stretch\_driver (i.e. the `/odom` topic), stretch\_body provides the same odometry as a dictionary of `base` called `status`. This is what it would look like:
```auto
import stretch_body.robot
r = stretch_body.robot.Robot()
r.startup()
print(r.base.status['x'], r.base.status['y'], r.base.status['theta'])
```
2. I can add support for velocity control of all joints to stretch\_driver. This might be preferred since stretch\_ros can accomplish more than stretch\_body can alone. For example, you can use the stretch\_navigation ROS pkg with the driver node to fuse measurements from the laser range finder, yielding a better odometry estimate than wheel encoders alone.
---
### Author:  [@CalaW](https://forum.hello-robot.com/u/CalaW)
#### Post date: [July 31, 2023, 9:59pm UTC](https://forum.hello-robot.com/t/integrate-velocity-control-into-ros/779/4 "2023-07-31T21:59:55Z")
Thanks Binit!
Yes, I prefer the second approach since I’m already using lidar fusion to get better odometry.
---
### Author:  [@bshah](https://forum.hello-robot.com/u/bshah)
#### Post date: [July 31, 2023, 10:12pm UTC](https://forum.hello-robot.com/t/integrate-velocity-control-into-ros/779/5 "2023-07-31T22:12:26Z")
Okay! I will begin adding this. I estimate it will take ~1 week, and I’ll post updates here about delays and/or when it becomes available.
---
### Author:  [@bshah](https://forum.hello-robot.com/u/bshah)
#### Post date: [August 4, 2023, 10:33am UTC](https://forum.hello-robot.com/t/integrate-velocity-control-into-ros/779/6 "2023-08-04T10:33:51Z")
Hi @CalaW, I’ve opened a [pull request](https://github.com/hello-robot/stretch_ros/pull/109) called “Velocity Control” that adds a new control mode called ‘velocity’ to the stretch\_driver node. When the driver is in this mode, the cmd\_vel topic still controls the mobile base, but now the FollowJointTrajectory action server will look at the velocities in the [JointTrajectoryPoint message](http://docs.ros.org/en/noetic/api/trajectory_msgs/html/msg/JointTrajectoryPoint.html) and command the robot’s joints accordingly. The positions array in the JointTrajectoryPoint message must be empty. I’ve also added a method that makes working with this new mode easier, called the `move_to_speed()` method. You can use it to send velocity commands to any joint like this:
```auto
# ensure driver is launched in a different terminal
$ ipython3
Python 3.8.10 (default, May 26 2023, 14:05:08)
Type 'copyright', 'credits' or 'license' for more information
IPython 8.7.0 -- An enhanced Interactive Python. Type '?' for help.
In [1]: import hello_helpers.hello_misc as hm
...: temp = hm.HelloNode.quick_create('temp')
[INFO] [1691144362.585790]: /temp started
[INFO] [1691144362.633690]: Node /temp connected to robot services.
In [2]: temp.move_at_speed({'joint_lift': 0.0})
In [3]: temp.move_at_speed({'joint_lift': 0.05})
In [4]: temp.move_at_speed({'joint_lift': -0.05})
In [5]: temp.move_at_speed({'joint_lift': 0.0})
In [6]: temp.move_at_speed({'joint_lift': 0.01, 'joint_arm': 0.01})
```
More details can be found in the PR. Note that there’s a few known bugs that will be fixed before the PR is merged. Also note that the gripper is still controlled with position commands (i.e. the [`move_to_pose()`](https://github.com/hello-robot/stretch_ros/tree/noetic/hello_helpers#move_to_posepose-return_before_donefalse-custom_contact_thresholdsfalse-custom_full_goalfalse) method).