def jnt_state_cb(self, msg):
velocity = {}
for name, vel in zip(msg.name, msg.velocity):
velocity[name] = vel
# lowpass for measured velocity
self.vel_trans = 0.5 * self.vel_trans + 0.5 * (velocity[
self.r_joint] + velocity[self.l_joint]) * self.wheel_radius / 2.0
self.vel_rot = 0.5 * self.vel_rot + 0.5 * (
velocity[self.r_joint] - velocity[self.l_joint]
) * self.wheel_radius / (2.0 * self.wheel_basis)
# velocity commands
vel_trans = self.vel_trans_desi + self.k_trans * (self.vel_trans_desi -
self.vel_trans)
vel_rot = self.vel_rot_desi + self.k_rot * (self.vel_rot_desi -
self.vel_rot)
# wheel commands
l_cmd = JointCommand()
l_cmd.name = self.l_joint
l_cmd.effort = self.vel_to_eff * (
vel_trans / self.wheel_radius -
vel_rot * self.wheel_basis / self.wheel_radius)
self.pub.publish(l_cmd)
r_cmd = JointCommand()
r_cmd.name = self.r_joint
r_cmd.effort = self.vel_to_eff * (
vel_trans / self.wheel_radius +
vel_rot * self.wheel_basis / self.wheel_radius)
self.pub.publish(r_cmd)
def jnt_state_cb(self, msg):
for name, pos, vel in zip(msg.name, msg.position, msg.velocity):
if name == self.name:
self.vel = 0.5 * self.vel + 0.5 * vel
if not self.initialized:
self.pos_desi = pos
self.initialized = True
cmd = JointCommand()
cmd.name = self.name
cmd.effort = 190.0 * (self.pos_desi - pos) - 4.0 * self.vel
print 'Joint at %f, going to %f, commanding joint %f' % (
pos, self.pos_desi, cmd.effort)
self.pub.publish(cmd)
def execute_cb(self, msg):
for i in range(0, len(msg.joint_names_goal)):
cmd = JointCommand()
cmd.name = msg.joint_names_goal[i]
cmd.effort = self.efforts[msg.joint_names_goal[i]]
rospy.logwarn(str(msg.joint_names_goal[i]))
rospy.logwarn(str(msg.joint_position_angles_goal[i]))
if msg.joint_position_angles_goal[i] < self.joint_positions[
msg.joint_names_goal[i]]:
cmd.effort = cmd.effort * -1
rospy.logwarn('publishe ' + str(cmd.effort) + ' auf ' +
str(cmd.name))
self.arm_joint_pub.publish(cmd)
while True:
if abs(msg.joint_position_angles_goal[i] -
self.joint_positions[msg.joint_names_goal[i]]
) > self.goal_threshold:
rospy.logwarn(
str(
abs(msg.joint_position_angles_goal[i] -
self.joint_positions[msg.joint_names_goal[i]]))
)
continue
else:
cmd.effort = 0.0
self.arm_joint_pub.publish(cmd)
time.sleep(.1)
self.arm_joint_pub.publish(cmd)
break
rospy.logwarn('reached')
self._result.joint_position_angles_result = [
self.joint_positions[MOTOR_A], self.joint_positions[MOTOR_B],
self.joint_positions[GRIPPER]
]
self._sas.set_succeeded(self._result)
rospy.logwarn("ArmPositionServer succeeded")
#import roslib; roslib.load_manifest('nxt_controllers')
import rospy
import math
import thread
from sensor_msgs.msg import JointState
from geometry_msgs.msg import Twist
from nxt_msgs.msg import Range, JointCommand, Contact
import time
import unittest
import multiprocessing
import threading
MOTOR_A = "motor_1"
MOTOR_B = "motor_2"
GRIPPER = "motor_3"
JOINT_0_COMMAND = JointCommand()
JOINT_1_COMMAND = JointCommand()
GRIPPER_COMMAND = JointCommand()
"""
Since during the test cases the "real" system is not running,
and thus also ROS is not running, all ROS relevant data be mocked.
For this purpose, the function pointers of the public and sleep functions
are overwritten with "empty" functions
"""
class mock_ros:
def publish(self, data):
pass
def sleep(self):
