Commit ef65260f authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

add extrusion capability for both robots simultaneously (#5)

parent b06e7e02
...@@ -4,27 +4,52 @@ ...@@ -4,27 +4,52 @@
<arg name="r2_kinematics_config" default="$(find twins_utils)/calibrations/r2_cal.yaml" doc="Kinematics config file used for calibration correction. This will be used to verify the robot's calibration is matching the robot_description."/> <arg name="r2_kinematics_config" default="$(find twins_utils)/calibrations/r2_cal.yaml" doc="Kinematics config file used for calibration correction. This will be used to verify the robot's calibration is matching the robot_description."/>
<arg name="r1_robot_ip" default="192.168.1.101"/> <arg name="r1_robot_ip" default="192.168.1.101"/>
<arg name="r2_robot_ip" default="192.168.1.103"/> <arg name="r2_robot_ip" default="192.168.1.103"/>
<arg name="extrude" default="True"/>
<!-- ROBOT 1 --> <!-- ROBOT 1 -->
<group ns="r1"> <group ns="r1">
<!-- ROBOT DRIVER -->
<include file="$(find ur_robot_driver)/launch/ur5e_bringup.launch"> <include file="$(find ur_robot_driver)/launch/ur5e_bringup.launch">
<arg name="robot_ip" value="$(arg r1_robot_ip)"/> <arg name="robot_ip" value="$(arg r1_robot_ip)"/>
<arg name="kinematics_config" value="$(arg r1_kinematics_config)"/> <arg name="kinematics_config" value="$(arg r1_kinematics_config)"/>
<arg name="reverse_port" value="50003"/> <arg name="reverse_port" value="50003"/>
<arg name="script_sender_port" value="50004"/> <arg name="script_sender_port" value="50004"/>
</include> </include>
<!-- MOVEIT WRAPPER -->
<include file="$(find ur5_e_moveit_config)/launch/ur5_e_moveit_planning_execution.launch"/> <include file="$(find ur5_e_moveit_config)/launch/ur5_e_moveit_planning_execution.launch"/>
<!-- EXTRUDER NODE-->
<node if="$(arg extrude)"
pkg="extrudex"
name="r1_extruder"
type="extruder_node.py"
args="/dev/ttyACM0"
output="screen" />
</group> </group>
<!-- ROBOT 2 --> <!-- ROBOT 2 -->
<group ns="r2"> <group ns="r2">
<!-- ROBOT DRIVER -->
<include file="$(find ur_robot_driver)/launch/ur5e_bringup.launch"> <include file="$(find ur_robot_driver)/launch/ur5e_bringup.launch">
<arg name="robot_ip" value="$(arg r2_robot_ip)"/> <arg name="robot_ip" value="$(arg r2_robot_ip)"/>
<arg name="kinematics_config" value="$(arg r2_kinematics_config)"/> <arg name="kinematics_config" value="$(arg r2_kinematics_config)"/>
<arg name="reverse_port" value="50001"/> <arg name="reverse_port" value="50001"/>
<arg name="script_sender_port" value="50002"/> <arg name="script_sender_port" value="50002"/>
</include> </include>
<!-- MOVEIT WRAPPER -->
<include file="$(find ur5_e_moveit_config)/launch/ur5_e_moveit_planning_execution.launch"/> <include file="$(find ur5_e_moveit_config)/launch/ur5_e_moveit_planning_execution.launch"/>
<!-- EXTRUDER NODE-->
<node if="$(arg extrude)"
pkg="extrudex"
name="r2_extruder"
type="extruder_node.py"
args="/dev/ttyACM1"
output="screen" />
</group> </group>
</launch> </launch>
...@@ -72,35 +72,40 @@ class Controller(object): ...@@ -72,35 +72,40 @@ class Controller(object):
robot can be 0 (if only one robot is turned on (not using 2xbringup), 1 for r1, 2 for r2 robot can be 0 (if only one robot is turned on (not using 2xbringup), 1 for r1, 2 for r2
""" """
## Initialise ROS things ## Initialise ROS things
self.node = rospy.init_node('py_controller', self.node = rospy.init_node('py_controller',
anonymous = False) anonymous = False)
self.extrusion_pub = rospy.Publisher('extrusion_cmd',ExtruderControl,
queue_size=2)
self.extruderReady_sub = rospy.Subscriber('ready',Bool,self._extruderReady_cb,
queue_size=1)
self.scaledPosTraj_pub = rospy.Publisher('scaled_pos_traj_controller/command',JointTrajectory, self.scaledPosTraj_pub = rospy.Publisher('scaled_pos_traj_controller/command',JointTrajectory,
queue_size=1) queue_size=1)
time.sleep(0.1)
if not disable_extruder:
print("Waiting for Hotend service")
rospy.wait_for_service(hotend_srv)
try:
self.extruderSetTemp_srv = rospy.ServiceProxy(hotend_srv,Hotend)
except rospy.ServiceException as e:
print('Service call failed: {}'.format(e))
# initialise MoveIt commander # initialise MoveIt commander
moveit_commander.roscpp_initialize(sys.argv) moveit_commander.roscpp_initialize(sys.argv)
if robot == 0: if robot == 0:
# MoveIt related
self.robot = moveit_commander.RobotCommander() self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface() self.scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander('manipulator') self.group = moveit_commander.MoveGroupCommander('manipulator')
self.motion_feedback_sub = rospy.Subscriber('scaled_pos_traj_controller/follow_joint_trajectory/result', FollowJointTrajectoryActionResult,
# Traj completion feedback
self.motion_feedback_sub = rospy.Subscriber(
'scaled_pos_traj_controller/follow_joint_trajectory/result',
FollowJointTrajectoryActionResult,
self._update_motion_status) self._update_motion_status)
# extruder related
self.extrusion_pub = rospy.Publisher(
'extrusion_cmd',
ExtruderControl,
queue_size=2)
self.extruderReady_sub = rospy.Subscriber(
'ready',
Bool,
self._extruderReady_cb,
queue_size=1)
else: else:
# MoveIt related
self.robot = moveit_commander.RobotCommander( self.robot = moveit_commander.RobotCommander(
"/r" + str(robot) + "/robot_description", "/r" + str(robot) + "/robot_description",
ns="r" + str(robot) ns="r" + str(robot)
...@@ -111,9 +116,36 @@ class Controller(object): ...@@ -111,9 +116,36 @@ class Controller(object):
robot_description="r" + str(robot) + "/robot_description", robot_description="r" + str(robot) + "/robot_description",
ns="r" + str(robot) ns="r" + str(robot)
) )
self.motion_feedback_sub = rospy.Subscriber("r" + str(robot) + '/scaled_pos_traj_controller/follow_joint_trajectory/result', FollowJointTrajectoryActionResult,
# Traj completion feedback
self.motion_feedback_sub = rospy.Subscriber(
"r" + str(robot) + '/scaled_pos_traj_controller/follow_joint_trajectory/result',
FollowJointTrajectoryActionResult,
self._update_motion_status) self._update_motion_status)
# Extruder related
self.extrusion_pub = rospy.Publisher(
"r" + str(robot) + '/extrusion_cmd',
ExtruderControl,
queue_size=2)
self.extruderReady_sub = rospy.Subscriber(
"r" + str(robot) + '/ready',
Bool,
self._extruderReady_cb,
queue_size=1)
hotend_srv = "/r" + str(robot) + '/hotend'
time.sleep(0.1)
if not disable_extruder:
print("Waiting for Hotend service")
print(hotend_srv)
rospy.wait_for_service(hotend_srv)
try:
self.extruderSetTemp_srv = rospy.ServiceProxy(hotend_srv,Hotend)
except rospy.ServiceException as e:
print('Service call failed: {}'.format(e))
self.group.set_max_velocity_scaling_factor(timeScale) self.group.set_max_velocity_scaling_factor(timeScale)
self.group.set_max_acceleration_scaling_factor(timeScale**2) self.group.set_max_acceleration_scaling_factor(timeScale**2)
......
...@@ -595,13 +595,13 @@ class KeyboardController(Controller): ...@@ -595,13 +595,13 @@ class KeyboardController(Controller):
if __name__=="__main__": if __name__=="__main__":
#con = KeyboardController(disableExtruder=True) #con = KeyboardController(disableExtruder=True)
con = KeyboardController() #con = KeyboardController()
#con1 = KeyboardController(disableExtruder=True, robot=1) #con1 = KeyboardController(robot=1)
#con2 = KeyboardController(disableExtruder=True, robot=2) con2 = KeyboardController(robot=2)
print('Collecting configrations...') print('Collecting configrations...')
configuations = con.prompt_configurations() #configuations = con.prompt_configurations()
#configuations = con1.prompt_configurations() #configuations = con1.prompt_configurations()
#configuations = con2.prompt_configurations() configuations = con2.prompt_configurations()
print('Your configurations:') print('Your configurations:')
print(configuations) print(configuations)
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment