def execute(self, userdata):
try:
physical_robot = rospy.get_parm('physical_robot')
except:
physical_robot = 'false'
if not physical_robot:
prc = subprocess.Popen("rosrun mbzirc_c2_auto idwrench2.py",
shell=True)
prc.wait()
if physical_robot:
prc = subprocess.Popen("rosrun mbzirc_c2_auto idwrench2.py",
shell=True)
prc.wait()
ret_state = rospy.get_param('smach_state')
return ret_state
def execute(self, userdata):
try:
physical_robot = rospy.get_parm('physical_robot')
except:
physical_robot = 'false'
if not physical_robot:
prc = subprocess.Popen("rosrun mbzirc_c2_auto idvalve.py",
shell=True)
prc.wait()
if physical_robot:
prc = subprocess.Popen("rosrun mbzirc_c2_auto idvalve.py",
shell=True)
prc.wait()
# smach_state will be set to valveNotFound, valveCenter, valveOffCenter
sm_state = rospy.get_param('smach_state')
if sm_state == "valveNotFound":
return 'valveNotFound'
else:
if sm_state == 'valveCenter':
userdata.valve_centered_out = True
else:
userdata.valve_centered_out = false
return 'valveLocated'
import rospy
import cv2
import numpy as np
import tf2_ros
import yaml
import tf
import roslib
roslib.load_manifest('transformations_in_ros')
if __name__ == '__main__':
euler_pitch = rospy.get_parm('pitch')
euler_yaw = rospy.get_parm('yaw')
euler_roll = rospy.get_parm('roll')
t_x = rospy.get_parm('x')
t_y = rospy.get_parm('y')
t_z = rospy.get_parm('z')
child = rospy.get_parm('child')
parent = rospy.get_parm('parent')
r = tf.transformations.quaternion_from_euler(euler_roll, euler_pitch,
euler_yaw)
print quaternion_val
yaml_stream = open('dump_test.yaml', 'w')
test_yaml = {
'test_yaml': {
'child_frame_id': child,
'header': {
'frame_id': parent
},
'transform': {
'translation': {
def execute(self, userdata):
rospy.Subscriber('/move_arm/valve_pos_kf', Twist, self.callback)
rospy.Subscriber('/gripper_status', Int8, self.callback_gripper)
rospy.sleep(0.1)
wrench_id = rospy.get_param('wrench_ID_m')
rospy.sleep(0.1)
ee_position = rospy.get_param('ee_position')
rospy.logdebug("wrench_id: [%f, %f, %f]", wrench_id[0], wrench_id[1],
wrench_id[2])
rospy.logdebug("ee_position: [%f, %f, %f]", ee_position[0],
ee_position[1], ee_position[2])
# Set the ready position 40 cm away from the wrenches
safety_dist = 0.40
wrench_id[0] = ee_position[0] + wrench_id[0] - safety_dist
wrench_id[1] = ee_position[1] + wrench_id[1]
wrench_id[2] = ee_position[2] + wrench_id[2] - 0.1
rospy.set_param(
'ee_position',
[float(wrench_id[0]),
float(wrench_id[1]),
float(wrench_id[2])])
rospy.set_param('move_arm_status', 'Pending')
move_state = moveArmTwist(wrench_id[0], wrench_id[1], wrench_id[2])
rospy.logdebug(
"Arm is centered on the wrench and 0.40m off the board.")
rospy.sleep(1)
dist_to_move = 0.0
try:
physical_robot = rospy.get_param('physical_robot')
except:
physical_robot = 'false'
rospy.sleep(0.1)
if not physical_robot:
moveUGVvel(0.05, 0.05, 'linear')
move_state = rospy.get_param('move_arm_status')
#wrench_id[0] = safety_dist #wrench_id[0]-dist_to_move
rospy.set_param('wrench_ID_m', wrench_id)
if not physical_robot:
"""
Open the gripper
Simulation
"""
gripper_pub = rospy.Publisher('gripper/cmd_vel',
Twist,
queue_size=1)
twist = Twist()
speed = .5
turn = 1
x = 0
y = 0
z = 0
th = 1 # To open gripper (1) use th = 1
twist.linear.x = x * speed
twist.linear.y = y * speed
twist.linear.z = z * speed
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = th * turn
ct = 0
rest_time = 0.1
tot_time = 3
while ct * rest_time < tot_time:
gripper_pub.publish(twist)
rospy.sleep(0.1)
ct = ct + 1
if physical_robot:
"""
Open the gripper
Physical
"""
gripper_pub_phys = rospy.Publisher('gripper_req',
Int8,
queue_size=1)
gripper_pub_phys.publish(int(1))
rospy.sleep(0.1)
gripper_status2 = 0
# Preset the out move counter to 0, override if necessary
userdata.move_counter_out = 0
if move_state == 'success':
rospy.sleep(0.1)
wrench_id = rospy.get_param('wrench_ID_m')
wrench_id[0]
rospy.logdebug("Wrench ID: [%f, %f, %f]", wrench_id[0],
wrench_id[1], wrench_id[2])
thresh = 10
xA = 20
ee_position = rospy.get_param('ee_position')
ct3 = 0
ee_position[0] = ee_position[0] + 0.1
kf = kf_msg()
kf_pub = rospy.Publisher("/move_arm/kf_init",
kf_msg,
queue_size=1,
latch=True)
ee_pub = rospy.Publisher("/move_arm/valve_pos",
Twist,
queue_size=1)
kf.initial_pos = [0, 0, 0, 0, 0, 0]
kf.initial_covar = [0.001, 0.001]
kf.covar_motion = [0.001, 0.001]
kf.covar_observer = [0.01, 0.01]
kf_pub.publish(kf)
rospy.sleep(0.1) # Wait for 0.1s to ensure init topic is published
rospy.sleep(1) # Wait for 1.0s to ensure init routine is completed
ee_twist = Twist()
if not physical_robot:
self.gripper_status = 0
self.gripper_status = 0
while self.gripper_status == 0 or self.gripper_status == 4:
#print "Gripper status: ", self.gripper_status
rospy.sleep(0.1)
rospy.loginfo("Gripper Status:")
rospy.loginfo(self.gripper_status)
try:
physical_robot = rospy.get_parm('physical_robot')
except:
physical_robot = 'false'
if not physical_robot:
prc = subprocess.Popen(
"rosrun mbzirc_c2_auto centerwrench.py", shell=True)
prc.wait()
if physical_robot:
prc = subprocess.Popen(
"rosrun mbzirc_c2_auto centerwrench.py", shell=True)
prc.wait()
rospy.sleep(0.1)
dist = rospy.get_param("wrench_ID_dist")
rospy.sleep(0.1)
wrench_id = rospy.get_param('wrench_ID_m')
rospy.sleep(0.1)
ee_position = rospy.get_param('ee_position')
rospy.sleep(0.1)
wrench_id_px = rospy.get_param('wrench_ID')
rospy.sleep(0.1)
rospy.logdebug("wrench_id: [%f, %f, %f]", wrench_id[0],
wrench_id[1], wrench_id[2])
rospy.logdebug("Wrench_id_px: [%f, %f]", wrench_id_px[0],
wrench_id_px[1])
dx = wrench_id[0] * 0.05
xA = rospy.get_param('xA')
# Set the ready position 40 cm away from the wrenches
#ee_position[0] = (xA + 0.4-0.14)+0.005*ct3 # 0.134 distance from camera to left_tip
ee_position[0] = ee_position[0] + 0.0025 * ct3
rospy.logdebug(
"ee_position before Kalman filter: [%f, %f, %f]",
ee_position[0], ee_position[1], ee_position[2])
rospy.logdebug(
"************************************************")
ee_twist.linear.x = ee_position[0]
ee_twist.linear.y = wrench_id[1]
ee_twist.linear.z = wrench_id[2]
ee_pub.publish(ee_twist)
rospy.sleep(0.1)
tw = self.valve_pos_kf
rospy.logdebug("Estimated pose from Kalman filter:")
rospy.logdebug("x = %f", tw.linear.x)
rospy.logdebug("y = %f", tw.linear.x)
rospy.logdebug("z = %f", tw.linear.x)
#ee_position[0] = ee_position[0]+0.005 #
ee_position[1] = ee_position[1] + tw.linear.y
ee_position[2] = ee_position[2] + tw.linear.z
rospy.set_param('ee_position', [
float(ee_position[0]),
float(ee_position[1]),
float(ee_position[2])
])
rospy.sleep(0.1)
rospy.set_param(
'wrench_ID_m',
[wrench_id[0] - dx, wrench_id[1], wrench_id[2]])
rospy.sleep(0.1)
rospy.logdebug(
"***********************************************")
rospy.logdebug("ee_position after Kalman filter: [%f, %f, %f]",
ee_position[0], ee_position[1], ee_position[2])
ee_twist.linear.y = ee_position[1]
ee_twist.linear.z = ee_position[2]
rospy.set_param('move_arm_status', 'Pending')
move_state = moveArmTwist(ee_twist.linear.x, ee_twist.linear.y,
ee_twist.linear.z)
#prc = subprocess.Popen("rosrun mbzirc_grasping move_arm_param.py", shell=True)
#prc.wait()
ct3 = ct3 + 1
if not physical_robot:
rospy.loginfo(
"***********************************************")
rospy.loginfo("ee_position:")
rospy.loginfo(ee_position[0])
rospy.loginfo("threshold:")
rospy.loginfo(xA + 0.461 - 0.170)
rospy.loginfo(
"***********************************************")
if ee_position[0] < xA + 0.461 - 0.170:
self.gripper_status = 0
else:
self.gripper_status = 1
rospy.logdebug("We are close enough!")
rospy.loginfo("Gripper is closing. Waiting for complete signal.")
ct4 = 0
if not physical_robot:
twist = Twist()
speed = .5
turn = 1
x = 0
y = 0
z = 0
th = -1 # To close gripper (1) use th = 1
twist.linear.x = x * speed
twist.linear.y = y * speed
twist.linear.z = z * speed
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = th * turn
ct = 0
rest_time = 0.1
tot_time = 3
while ct * rest_time < tot_time:
gripper_pub.publish(twist)
rospy.sleep(0.1)
ct = ct + 1
self.gripper_status = 2
while self.gripper_status != 2:
rospy.sleep(0.1)
rospy.logdebug("Gripper Status:")
rospy.logdebug(self.gripper_status)
ct4 = ct4 + 1
if ct4 > 100:
"""
gripper_pub_phys = rospy.Publisher('gripper_req', Int8, queue_size = 1)
gripper_pub_phys.publish(int(0))
rospy.sleep(1)
gripper_pub_phys.publish(int(1))
rospy.sleep(1)
ct4 = 0
"""
self.gripper_status = 2
rospy.loginfo("Gripper is closed.")
return 'atWrench'
else:
if userdata.move_counter_in < userdata.max_retries:
userdata.move_counter_out = userdata.move_counter_in + 1
return 'moveStuck'
else:
return 'moveFailed'