def sendDefaultHeadConfig(self):
ps = JointState()
ps.name = 'head_pan_joint'
ps.position = 0.0
ts = JointState()
ts.name = 'head_tilt_joint'
ts.position = 0.0
js = JointStates()
js.joints = [ps, ts]
self.pub.publish(js)
def sendHeadConfig(self):
ps = JointState()
ps.name = 'head_pan_joint'
ps.position = self.configs_[self.current_config_][0]
ts = JointState()
ts.name = 'head_tilt_joint'
ts.position = self.configs_[self.current_config_][1]
js = JointStates()
js.joints = [ps, ts]
self.pub.publish(js)
def sendHeadConfig(self):
if self.status.value != robot_actions.msg.ActionStatus.ACTIVE:
return
ps = JointState()
ps.name = 'head_pan_joint'
ps.position = self.head_configs_[self.current_config_][0]
ts = JointState()
ts.name = 'head_tilt_joint'
ts.position = self.head_configs_[self.current_config_][1]
js = JointStates()
js.joints = [ps, ts]
self.head_pub_.publish(js)
def point_head_client(pan, tilt):
ps = JointState()
ps.name = 'head_pan_joint'
ps.position = pan
ts = JointState()
ts.name ='head_tilt_joint'
ts.position = tilt
js = JointStates()
js.joints = [ps, ts]
head_angles.publish(js)
sleep(0.5)
def moveArmState(self, joints):
msg = MoveArmGoal()
msg.implicit_goal = 0
msg.goal_configuration = []
msg.goal_constraints = []
for j in joints:
msg.goal_configuration.append(
JointState(j, joints[j], 0.0, 0.0, 0.0, 0))
msg.enable = 1
msg.timeout = 0.0
self.pub.publish(msg)
print '[MoveArm] Sending arm to: ' + ` msg.goal_configuration `
# HACK to get around the lack of proper goal_id support
print '[MoveArm] Waiting for goal to be taken up...'
rospy.sleep(2.0)
while self.status == None or self.status.value == ControllerStatus.ACTIVE:
print '[MoveArm] Waiting for goal achievement...'
rospy.sleep(1.0)
return self.status.value == ControllerStatus.SUCCESS
#/laser_tilt_controller/laser_scanner_signal - better!
capture_waiter = WaitForKMessagesAdapter("wait_k_messages_action", 3,
10)
#capture_configs=[[0.0,-0.1],[-0.5, 0.3],[0.5, 0.3]];
#multi_config_waiter=WaitForMultipleHeadConfigsAdapter(capture_configs,capture_waiter);
move_head_adapter = MoveHeadAdapter("/move_head/move_head_action", -1)
move_head_adapter2 = MoveHeadAdapter("/move_head2/move_head_action",
-1)
hc_goal_topic_ = "/head_controller/command"
hc_pub = rospy.Publisher(hc_goal_topic_, JointStates)
ps = JointState()
ps.name = 'head_pan_joint'
ps.position = 0.0
ts = JointState()
ts.name = 'head_tilt_joint'
ts.position = 0.3
js = JointStates()
js.joints = [ps, ts]
hc_pub.publish(js)
rospy.sleep(0.1)
hc_pub.publish(js)
chrg_stations = [[[18.098, 21.015, 0.000],
[0.000, 0.000, 0.713, 0.701]]]