You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
208 lines
9.6 KiB
208 lines
9.6 KiB
2 months ago
|
#!/usr/bin/env python
|
||
|
|
|
||
|
|
import rospy
|
||
|
|
import rosnode
|
||
|
|
from std_msgs.msg import Bool, UInt8
|
||
|
|
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
|
||
|
|
from geometry_msgs.msg import Twist
|
||
|
|
from geometry_msgs.msg import PoseArray, Pose
|
||
|
|
|
||
|
|
|
||
|
|
class CmdCommutator(object):
|
||
|
|
def __init__(self) -> None:
|
||
|
|
self.cur_device = 0
|
||
|
|
|
||
|
|
rospy.init_node("cmd_commutator")
|
||
|
|
|
||
|
|
freq = rospy.get_param('~frequency', 500)
|
||
|
|
|
||
|
|
# read config file
|
||
|
|
output_cmd_vel_topic = rospy.get_param('~output_cmd_vel_topic', "cmd_vel")
|
||
|
|
output_cmd_pose_topic = rospy.get_param('~output_cmd_pose_topic', "cmd_pose")
|
||
|
|
output_cmd_ef_pose_topic = rospy.get_param('~output_cmd_ef_pose_topic', "ef_position/command")
|
||
|
|
output_cmd_joint_pos_topic = rospy.get_param('~output_cmd_joint_pos_topic', "joint_group_position_controller/command")
|
||
|
|
output_status_topic = rospy.get_param('~output_status_topic', "cur_device")
|
||
|
|
|
||
|
|
input1_cmd_vel_topic = rospy.get_param('~input1_cmd_vel_topic', "ds4/cmd_vel")
|
||
|
|
input1_cmd_pose_topic = rospy.get_param('~input1_cmd_pose_topic', "ds4/cmd_pose")
|
||
|
|
input1_cmd_ef_pose_topic = rospy.get_param('~input1_cmd_ef_pose_topic', "ds4/ef_position/command")
|
||
|
|
input1_cmd_joint_pos_topic = rospy.get_param('~input1_cmd_joint_pos_topic', "ds4/joint_group_position_controller/command")
|
||
|
|
input1_status_topic = rospy.get_param('~input1_status_topic', "ds4/status")
|
||
|
|
|
||
|
|
input2_cmd_vel_topic = rospy.get_param('~input2_cmd_vel_topic', "radiolink/cmd_vel")
|
||
|
|
input2_cmd_pose_topic = rospy.get_param('~input2_cmd_pose_topic', "radiolink/cmd_pose")
|
||
|
|
input2_cmd_ef_pose_topic = rospy.get_param('~input2_cmd_ef_pose_topic', "radiolink/ef_position/command")
|
||
|
|
input2_cmd_joint_pos_topic = rospy.get_param('~input2_cmd_joint_pos_topic', "radiolink/joint_group_position_controller/command")
|
||
|
|
input2_status_topic = rospy.get_param('~input2_status_topic', "radiolink/status")
|
||
|
|
|
||
|
|
input3_cmd_vel_topic = rospy.get_param('~input3_cmd_vel_topic', "nav/cmd_vel")
|
||
|
|
input3_cmd_pose_topic = rospy.get_param('~input3_cmd_pose_topic', "nav/cmd_pose")
|
||
|
|
input3_cmd_ef_pose_topic = rospy.get_param('~input3_cmd_ef_pose_topic', "nav/ef_position/command")
|
||
|
|
input3_cmd_joint_pos_topic = rospy.get_param('~input3_cmd_joint_pos_topic', "nav/joint_group_position_controller/command")
|
||
|
|
input3_status_topic = rospy.get_param('~input3_status_topic', "nav/status")
|
||
|
|
|
||
|
|
input4_cmd_vel_topic = rospy.get_param('~input4_cmd_vel_topic', "gui/cmd_vel")
|
||
|
|
input4_cmd_pose_topic = rospy.get_param('~input4_cmd_pose_topic', "gui/cmd_pose")
|
||
|
|
input4_cmd_ef_pose_topic = rospy.get_param('~input4_cmd_ef_pose_topic', "gui/ef_position/command")
|
||
|
|
input4_cmd_joint_pos_topic = rospy.get_param('~input4_cmd_joint_pos_topic', "gui/joint_group_position_controller/command")
|
||
|
|
input4_status_topic = rospy.get_param('~input4_status_topic', "gui/status")
|
||
|
|
|
||
|
|
self.rate = rospy.Rate(freq)
|
||
|
|
|
||
|
|
# subscribers
|
||
|
|
rospy.Subscriber(input1_cmd_vel_topic, Twist, self.input1_cmd_vel_callback, queue_size=1)
|
||
|
|
rospy.Subscriber(input1_cmd_pose_topic, Twist, self.input1_cmd_pose_callback, queue_size=1)
|
||
|
|
rospy.Subscriber(input1_cmd_ef_pose_topic, PoseArray, self.input1_cmd_ef_pose_callback, queue_size=1)
|
||
|
|
rospy.Subscriber(input1_cmd_joint_pos_topic, JointTrajectoryPoint, self.input1_cmd_joint_pos_callback, queue_size=1)
|
||
|
|
rospy.Subscriber(input1_status_topic, Bool, self.input1_status_callback, queue_size=1)
|
||
|
|
|
||
|
|
rospy.Subscriber(input2_cmd_vel_topic, Twist, self.input2_cmd_vel_callback, queue_size=1)
|
||
|
|
rospy.Subscriber(input2_cmd_pose_topic, Twist, self.input2_cmd_pose_callback, queue_size=1)
|
||
|
|
rospy.Subscriber(input2_cmd_ef_pose_topic, PoseArray, self.input2_cmd_ef_pose_callback, queue_size=1)
|
||
|
|
rospy.Subscriber(input2_cmd_joint_pos_topic, JointTrajectoryPoint, self.input2_cmd_joint_pos_callback, queue_size=1)
|
||
|
|
rospy.Subscriber(input2_status_topic, Bool, self.input2_status_callback, queue_size=1)
|
||
|
|
|
||
|
|
rospy.Subscriber(input3_cmd_vel_topic, Twist, self.input3_cmd_vel_callback, queue_size=1)
|
||
|
|
rospy.Subscriber(input3_cmd_pose_topic, Twist, self.input3_cmd_pose_callback, queue_size=1)
|
||
|
|
rospy.Subscriber(input3_cmd_ef_pose_topic, PoseArray, self.input3_cmd_ef_pose_callback, queue_size=1)
|
||
|
|
rospy.Subscriber(input3_cmd_joint_pos_topic, JointTrajectoryPoint, self.input3_cmd_joint_pos_callback, queue_size=1)
|
||
|
|
rospy.Subscriber(input3_status_topic, Bool, self.input3_status_callback, queue_size=1)
|
||
|
|
|
||
|
|
rospy.Subscriber(input4_cmd_vel_topic, Twist, self.input4_cmd_vel_callback, queue_size=1)
|
||
|
|
rospy.Subscriber(input4_cmd_pose_topic, Twist, self.input4_cmd_pose_callback, queue_size=1)
|
||
|
|
rospy.Subscriber(input4_cmd_ef_pose_topic, PoseArray, self.input4_cmd_ef_pose_callback, queue_size=1)
|
||
|
|
rospy.Subscriber(input4_cmd_joint_pos_topic, JointTrajectoryPoint, self.input4_cmd_joint_pos_callback, queue_size=1)
|
||
|
|
rospy.Subscriber(input4_status_topic, Bool, self.input4_status_callback, queue_size=1)
|
||
|
|
|
||
|
|
# publishers
|
||
|
|
self.cmd_vel_pub = rospy.Publisher(output_cmd_vel_topic, Twist, queue_size=10)
|
||
|
|
self.cmd_pose_pub = rospy.Publisher(output_cmd_pose_topic, Twist, queue_size=10)
|
||
|
|
self.cmd_ef_pos_pub = rospy.Publisher(output_cmd_ef_pose_topic, PoseArray, queue_size=10)
|
||
|
|
self.cmd_joint_pos_pub = rospy.Publisher(output_cmd_joint_pos_topic, JointTrajectoryPoint, queue_size=10)
|
||
|
|
self.status_pub = rospy.Publisher(output_status_topic, UInt8, queue_size=10)
|
||
|
|
|
||
|
|
# messages
|
||
|
|
self.cmd_vel_msg = Twist()
|
||
|
|
self.cmd_pose_msg = Twist()
|
||
|
|
self.cmd_ef_msg = PoseArray()
|
||
|
|
self.cmd_joint_msg = JointTrajectoryPoint()
|
||
|
|
|
||
|
|
|
||
|
|
self.input1_status = False
|
||
|
|
self.input2_status = False
|
||
|
|
self.input3_status = False
|
||
|
|
self.input4_status = False
|
||
|
|
|
||
|
|
rospy.loginfo("cmd_commutator configured")
|
||
|
|
|
||
|
|
def run(self):
|
||
|
|
while not rospy.is_shutdown():
|
||
|
|
if rosnode.rosnode_ping("/move_base", max_count=2, verbose=False):
|
||
|
|
if self.input1_status == False and self.input2_status == False:
|
||
|
|
self.cur_device = 3
|
||
|
|
else:
|
||
|
|
if self.input1_status == False and self.input2_status == False and self.input4_status == True:
|
||
|
|
self.cur_device = 4
|
||
|
|
self.input3_status = False
|
||
|
|
|
||
|
|
if self.input1_status == True:
|
||
|
|
self.cur_device = 1
|
||
|
|
elif self.input2_status == True:
|
||
|
|
self.cur_device = 2
|
||
|
|
elif self.input3_status == True:
|
||
|
|
self.cur_device = 3
|
||
|
|
elif self.input4_status == True:
|
||
|
|
self.cur_device = 4
|
||
|
|
else:
|
||
|
|
self.cur_device = 0
|
||
|
|
self.cmd_vel_pub.publish(self.cmd_vel_msg)
|
||
|
|
self.cmd_pose_pub.publish(self.cmd_pose_msg)
|
||
|
|
self.cmd_ef_pos_pub.publish(self.cmd_ef_msg)
|
||
|
|
self.cmd_joint_pos_pub.publish(self.cmd_joint_msg)
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
self.status_pub.publish(self.cur_device)
|
||
|
|
self.rate.sleep()
|
||
|
|
|
||
|
|
def input1_cmd_vel_callback(self, msg : Twist):
|
||
|
|
if self.cur_device == 1:
|
||
|
|
self.cmd_vel_pub.publish(msg)
|
||
|
|
|
||
|
|
def input1_cmd_pose_callback(self, msg : Twist):
|
||
|
|
if self.cur_device == 1:
|
||
|
|
self.cmd_pose_pub.publish(msg)
|
||
|
|
|
||
|
|
def input1_cmd_ef_pose_callback(self, msg : PoseArray):
|
||
|
|
if self.cur_device == 1:
|
||
|
|
self.cmd_ef_pos_pub.publish(msg)
|
||
|
|
|
||
|
|
def input1_cmd_joint_pos_callback(self, msg : JointTrajectoryPoint):
|
||
|
|
if self.cur_device == 1:
|
||
|
|
self.cmd_joint_pos_pub.publish(msg)
|
||
|
|
|
||
|
|
def input1_status_callback(self, msg : Bool):
|
||
|
|
self.input1_status = msg.data
|
||
|
|
|
||
|
|
def input2_cmd_vel_callback(self, msg : Twist):
|
||
|
|
if self.cur_device == 2:
|
||
|
|
self.cmd_vel_pub.publish(msg)
|
||
|
|
|
||
|
|
def input2_cmd_pose_callback(self, msg : Twist):
|
||
|
|
if self.cur_device == 2:
|
||
|
|
self.cmd_pose_pub.publish(msg)
|
||
|
|
|
||
|
|
def input2_cmd_ef_pose_callback(self, msg : PoseArray):
|
||
|
|
if self.cur_device == 2:
|
||
|
|
self.cmd_ef_pos_pub.publish(msg)
|
||
|
|
|
||
|
|
def input2_cmd_joint_pos_callback(self, msg : JointTrajectoryPoint):
|
||
|
|
if self.cur_device == 2:
|
||
|
|
self.cmd_joint_pos_pub.publish(msg)
|
||
|
|
|
||
|
|
def input2_status_callback(self, msg : Bool):
|
||
|
|
self.input2_status = msg.data
|
||
|
|
|
||
|
|
def input3_cmd_vel_callback(self, msg : Twist):
|
||
|
|
if self.cur_device == 3:
|
||
|
|
self.cmd_vel_pub.publish(msg)
|
||
|
|
|
||
|
|
def input3_cmd_pose_callback(self, msg : Twist):
|
||
|
|
if self.cur_device == 3:
|
||
|
|
self.cmd_pose_pub.publish(msg)
|
||
|
|
|
||
|
|
def input3_cmd_ef_pose_callback(self, msg : PoseArray):
|
||
|
|
if self.cur_device == 3:
|
||
|
|
self.cmd_ef_pos_pub.publish(msg)
|
||
|
|
|
||
|
|
def input3_cmd_joint_pos_callback(self, msg : JointTrajectoryPoint):
|
||
|
|
if self.cur_device == 3:
|
||
|
|
self.cmd_joint_pos_pub.publish(msg)
|
||
|
|
|
||
|
|
def input3_status_callback(self, msg : Bool):
|
||
|
|
self.input3_status = msg.data
|
||
|
|
|
||
|
|
def input4_cmd_vel_callback(self, msg : Twist):
|
||
|
|
if self.cur_device == 4:
|
||
|
|
self.cmd_vel_pub.publish(msg)
|
||
|
|
|
||
|
|
def input4_cmd_pose_callback(self, msg : Twist):
|
||
|
|
if self.cur_device == 4:
|
||
|
|
self.cmd_pose_pub.publish(msg)
|
||
|
|
|
||
|
|
def input4_cmd_ef_pose_callback(self, msg : PoseArray):
|
||
|
|
if self.cur_device == 4:
|
||
|
|
self.cmd_ef_pos_pub.publish(msg)
|
||
|
|
|
||
|
|
def input4_cmd_joint_pos_callback(self, msg : JointTrajectoryPoint):
|
||
|
|
if self.cur_device == 4:
|
||
|
|
self.cmd_joint_pos_pub.publish(msg)
|
||
|
|
|
||
|
|
def input4_status_callback(self, msg : Bool):
|
||
|
|
self.input4_status = msg.data
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == '__main__':
|
||
|
|
cmd_com = CmdCommutator()
|
||
|
|
cmd_com.run()
|