def steering_state_cb(data):
js = JointState()
js.name = data.name
js.motor_ids = data.motor_ids
js.current_pos = data.current_pos
self.steer_state[js.motor_ids[0]] = js
def _HandleJoint_1_Command(self, data):
""" republish position. """
neck = (data)
self.gear_ratio = 0.3
new_pos = float(data.current_pos) * self.gear_ratio
new_goal = float(data.goal_pos) * self.gear_ratio
new_error = new_goal - new_pos
jointstate = JointState()
jointstate.name = ("head_tilt_mod_joint")
jointstate.motor_ids = (neck.motor_ids)
jointstate.motor_temps = (neck.motor_temps)
jointstate.goal_pos = (new_goal)
jointstate.current_pos = (new_pos)
jointstate.velocity = (neck.velocity)
jointstate.load = (neck.load)
jointstate.is_moving = (neck.is_moving)
jointstate.error = (new_error)
#rospy.loginfo(jointstate)
self._neckPublisher.publish(jointstate)
def _HandleJoint_1_Command(self, data):
""" republish position. """
neck =(data)
self.gear_ratio = 0.3
new_pos =float (data.current_pos) * self.gear_ratio
new_goal=float (data.goal_pos) * self.gear_ratio
new_error = new_goal - new_pos
jointstate = JointState()
jointstate.name =("head_tilt_mod_joint")
jointstate.motor_ids =(neck.motor_ids)
jointstate.motor_temps =(neck.motor_temps)
jointstate.goal_pos =(new_goal)
jointstate.current_pos =(new_pos)
jointstate.velocity =(neck.velocity)
jointstate.load =(neck.load)
jointstate.is_moving =(neck.is_moving)
jointstate.error =(new_error)
#rospy.loginfo(jointstate)
self._neckPublisher.publish(jointstate)
