yikestone/ros_arduino_bridge
1
0
Fork 0
mirror of https://github.com/YikeStone/ros_arduino_bridge.git synced 2025-08-05 20:14:07 +05:30
Code Issues Projects Releases Wiki Activity

Finished tweaks on follow_controller.py to execute joint action trajectories

Browse Source
This commit is contained in:
Patrick Goebel 2016-11-06 06:08:37 -08:00
parent d9968a7f4b
commit 6d6390fa53
2 changed files with 13 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ros_arduino_python/src/ros_arduino_python/follow_controller.py
View File

@ -61,7 +61,7 @@ class FollowController(Controller):
def execute_cb(self, goal): def execute_cb(self, goal):
rospy.loginfo(self.name + ": Action goal recieved.") rospy.loginfo(self.name + ": Action goal recieved.")
traj = goal.trajectory traj = goal.trajectory
if set(self.joints) != set(traj.joint_names): if set(self.joints) != set(traj.joint_names):
for j in self.joints: for j in self.joints:
if j not in traj.joint_names: if j not in traj.joint_names:
@ -115,6 +115,10 @@ class FollowController(Controller):
start = traj.header.stamp start = traj.header.stamp
if start.secs == 0 and start.nsecs == 0: if start.secs == 0 and start.nsecs == 0:
start = rospy.Time.now() start = rospy.Time.now()
# Set the start time for each joint
for i in range(len(self.joints)):
self.device.joints[self.joints[i]].time_move_started = start
r = rospy.Rate(self.rate) r = rospy.Rate(self.rate)
last = [ self.device.joints[joint].position for joint in self.joints ] last = [ self.device.joints[joint].position for joint in self.joints ]
@ -138,8 +142,12 @@ class FollowController(Controller):
last[i] += cmd last[i] += cmd
self.device.joints[self.joints[i]].time_move_started = rospy.Time.now() self.device.joints[self.joints[i]].time_move_started = rospy.Time.now()
self.device.joints[self.joints[i]].in_trajectory = True self.device.joints[self.joints[i]].in_trajectory = True
self.device.joints[self.joints[i]].is_moving = True
self.device.joints[self.joints[i]].desired = last[i] self.device.joints[self.joints[i]].desired = last[i]
else: else:
self.device.joints[self.joints[i]].speed = 0.0
self.device.joints[self.joints[i]].in_trajectory = True
self.device.joints[self.joints[i]].is_moving = False
velocity[i] = 0 velocity[i] = 0
r.sleep() r.sleep()
return True return True

28
ros_arduino_python/src/ros_arduino_python/servo_controller.py
View File

@ -36,7 +36,6 @@ class Joint:
self.position = 0.0 # radians self.position = 0.0 # radians
self.start_position = 0.0 self.start_position = 0.0
self.goal_position = 0.0
self.position_last = 0.0 # radians self.position_last = 0.0 # radians
self.velocity = 0.0 # rad/s self.velocity = 0.0 # rad/s
self.servo_speed = 0.0 # rad/s self.servo_speed = 0.0 # rad/s
@ -144,9 +143,6 @@ class Servo(Joint):
# Record the starting position # Record the starting position
self.start_position = self.get_current_position() self.start_position = self.get_current_position()
# Record the goal_postion
self.goal_position = msg.data
# Set the target position for the next servo controller update # Set the target position for the next servo controller update
self.desired = msg.data self.desired = msg.data
@ -280,24 +276,6 @@ class ContinousServo(Joint):
self.is_moving = True self.is_moving = True
self.direction = copysign(1, self.desired - self.position) self.direction = copysign(1, self.desired - self.position)
# def command_cb(self, msg):
# # Check limits
# if msg.data > self.max_speed:
# msg.data = self.max_speed
#
# if msg.data < self.min_speed:
# msg.data = self.min_speed
#
# # Adjust for the neutral offset
# if self.invert:
# servo_ticks = self.neutral - msg.data * self.ticks_per_rad_per_sec
# else:
# servo_ticks = self.neutral + msg.data * self.ticks_per_rad_per_sec
#
# # Set the target speed for the next servo controller update
# self.desired = radians(servo_ticks)
class ServoController(): class ServoController():
def __init__(self, device, name): def __init__(self, device, name):
self.name = name self.name = name
@ -317,6 +295,7 @@ class ServoController():
if rospy.Time.now() > self.next_update: if rospy.Time.now() > self.next_update:
for servo in self.servos: for servo in self.servos:
# Check to see if we are within 2 degrees of the desired position or gone past it # Check to see if we are within 2 degrees of the desired position or gone past it
if not servo.in_trajectory and servo.is_moving: if not servo.in_trajectory and servo.is_moving:
if abs(servo.position - servo.desired) < radians(2.0) or copysign(1, servo.desired - servo.position) != servo.direction: if abs(servo.position - servo.desired) < radians(2.0) or copysign(1, servo.desired - servo.position) != servo.direction:
@ -324,11 +303,12 @@ class ServoController():
servo.is_moving = False servo.is_moving = False
servo.velocity = 0.0 servo.velocity = 0.0
duration = rospy.Time.now() - servo.time_move_started duration = rospy.Time.now() - servo.time_move_started
# If the servo is still moving, update its interpolated position and velocity # If the servo is still moving, update its interpolated position and velocity
if servo.is_moving: if servo.is_moving:
try: try:
# Get the interpolated current position of the servo # Get the interpolated current position of the servo
servo.position = servo.get_interpolated_position()
if not servo.in_trajectory: if not servo.in_trajectory:
servo.position = servo.get_interpolated_position() servo.position = servo.get_interpolated_position()
else: else:
@ -337,7 +317,7 @@ class ServoController():
# We cannot interpolate both position and velocity so just set velocity to the current speed # We cannot interpolate both position and velocity so just set velocity to the current speed
servo.velocity = servo.servo_speed servo.velocity = servo.servo_speed
servo.position_last = servo.position servo.position_last = servo.position
# Update diagnostics counters # Update diagnostics counters
servo.diagnostics.reads += 1 servo.diagnostics.reads += 1
servo.diagnostics.total_reads += 1 servo.diagnostics.total_reads += 1