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: