def disable_collision(self):
# before disabling edo_algorithms we need to "disable" collision checking on joints and raspberry
msg_dis_coll_js = JointInit()
msg_dis_coll_js.mode = 3
msg_dis_coll_js.joints_mask = 63
msg_dis_coll_js.reduction_factor = 10.0
msg_dis_coll_js_m = JointInit()
msg_dis_coll_js_m.mode = 3
msg_dis_coll_js_m.joints_mask = 127
msg_dis_coll_js_m.reduction_factor = 10.0
msg_dis_coll_algo = CollisionThreshold()
msg_dis_coll_algo.joints_mask = 127
msg_dis_coll_algo.threshold = 10.0
self._joint_init_command_pub.publish(msg_dis_coll_js)
self._machine_init_pub = rospy.Publisher('/machine_init',
JointInit,
queue_size=1,
latch=True)
self._machine_init_pub.publish(msg_dis_coll_js_m)
self._algo_col_thr_pub = rospy.Publisher('/algo_coll_thr',
CollisionThreshold,
queue_size=1,
latch=True)
self._algo_col_thr_pub.publish(msg_dis_coll_algo)
def listen():
#########################################
#########################################
# Subscribe to know if there is a curent move
#rospy.Subscriber("/move_in_progress", Bool, callback_move)
# if no current move :
# execute the move
# else pass
#########################################
#########################################
rospy.init_node('cs_avenir_controller', anonymous=True)
# to calibrate the collision system
machine_init = rospy.Publisher('machine_init',
JointInit,
queue_size=10,
latch=True)
# first param: mode 3 - manage collision thresold
# second param: axes mask (63 affect all axes)
# third param: current threshold
imsg = JointInit()
imsg.mode = 3
imsg.joints_mask = 127 # all but the gripper
imsg.reduction_factor = 100.0
# publish init to remove collision
machine_init.publish(imsg)
# listen if a button is pushed and get the number
rospy.Subscriber("cs_avenir/buttons", UInt8, callback)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
def select_6_axis_with_gripper_edo(self):
rospy.loginfo("Trying to select 6-axis robot with a gripper...")
# selecting 6 axis robot
msg_ji = JointInit()
# this code is from ros.service.ts
msg_ji.mode = 0
msg_ji.joints_mask = (1 << self.NUMBER_OF_JOINTS) - 1 # 127
msg_ji.reduction_factor = 0.0
# rospy.loginfo(msg_ji)
self._joint_init_command_pub.publish(msg_ji) # /bridge_init
rospy.loginfo("Message sent, 6-axis configuration was selected")
def listen():
rospy.init_node('sub_test', anonymous=True)
machine_init = rospy.Publisher('machine_init', JointInit, queue_size=10, latch=True)
# first param: mode 3 - manage collision thresold
# second param: axes mask (63 affect all axes)
# third param: current threshold
imsg = JointInit()
imsg.mode = 3
imsg.joints_mask = 127
imsg.reduction_factor = 100.0
# publish init to remove collision
machine_init.publish(imsg)
rospy.Subscriber("cs_avenir/buttons", UInt8, callback)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
float(parsed[4]),
float(parsed[5])
]
bit_flag = 63
#machine_move_pub = rospy.Publisher('machine_move', MovementCommand)
#print mmmsg
ros_cartesian_pose = CartesianPose(0.0,0.0,0.0,0.0,0.0,0.0,'')
ros_point = Point(74, ros_cartesian_pose, bit_flag, joints)
ros_frame = Frame(0.0,0.0,0.0,0.0,0.0,0.0)
mmmsg = MovementCommand(77, 74, 25, 0, 0, 0.0, ros_point, ros_point, ros_frame, ros_frame)
# publish move
machine_move.publish(mmmsg)
imsg = JointInit(3, 127, 10.0)
# publish init to remove collision
machine_init.publish(imsg)
while waitme == 0:
time.sleep(1)
#print("after waitme")
control_switch(1)
try:
os.system("kill $(ps aux | grep 'edo_algorithms' | awk '{print $2}')")
except:
print ("edo_algorithm not running")
while not rospy.is_shutdown() and startc5g == 1: