Commit ef65260f authored by Jayant Khatkar's avatar Jayant Khatkar

add extrusion capability for both robots simultaneously (#5)

parent b06e7e02
......@@ -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="r1_robot_ip" default="192.168.1.101"/>
<arg name="r2_robot_ip" default="192.168.1.103"/>
<arg name="extrude" default="True"/>
<!-- ROBOT 1 -->
<group ns="r1">
<!-- ROBOT DRIVER -->
<include file="$(find ur_robot_driver)/launch/ur5e_bringup.launch">
<arg name="robot_ip" value="$(arg r1_robot_ip)"/>
<arg name="kinematics_config" value="$(arg r1_kinematics_config)"/>
<arg name="reverse_port" value="50003"/>
<arg name="script_sender_port" value="50004"/>
</include>
<!-- MOVEIT WRAPPER -->
<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>
<!-- ROBOT 2 -->
<group ns="r2">
<!-- ROBOT DRIVER -->
<include file="$(find ur_robot_driver)/launch/ur5e_bringup.launch">
<arg name="robot_ip" value="$(arg r2_robot_ip)"/>
<arg name="kinematics_config" value="$(arg r2_kinematics_config)"/>
<arg name="reverse_port" value="50001"/>
<arg name="script_sender_port" value="50002"/>
</include>
<!-- MOVEIT WRAPPER -->
<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>
</launch>
......@@ -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
"""
## Initialise ROS things
self.node = rospy.init_node('py_controller',
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,
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
moveit_commander.roscpp_initialize(sys.argv)
if robot == 0:
# MoveIt related
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander('manipulator')
self.motion_feedback_sub = rospy.Subscriber('scaled_pos_traj_controller/follow_joint_trajectory/result', FollowJointTrajectoryActionResult,
self._update_motion_status)
# Traj completion feedback
self.motion_feedback_sub = rospy.Subscriber(
'scaled_pos_traj_controller/follow_joint_trajectory/result',
FollowJointTrajectoryActionResult,
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:
# MoveIt related
self.robot = moveit_commander.RobotCommander(
"/r" + str(robot) + "/robot_description",
ns="r" + str(robot)
......@@ -111,8 +116,35 @@ class Controller(object):
robot_description="r" + str(robot) + "/robot_description",
ns="r" + str(robot)
)
self.motion_feedback_sub = rospy.Subscriber("r" + str(robot) + '/scaled_pos_traj_controller/follow_joint_trajectory/result', FollowJointTrajectoryActionResult,
self._update_motion_status)
# 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)
# 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_acceleration_scaling_factor(timeScale**2)
......
......@@ -595,13 +595,13 @@ class KeyboardController(Controller):
if __name__=="__main__":
#con = KeyboardController(disableExtruder=True)
con = KeyboardController()
#con1 = KeyboardController(disableExtruder=True, robot=1)
#con2 = KeyboardController(disableExtruder=True, robot=2)
#con = KeyboardController()
#con1 = KeyboardController(robot=1)
con2 = KeyboardController(robot=2)
print('Collecting configrations...')
configuations = con.prompt_configurations()
#configuations = con.prompt_configurations()
#configuations = con1.prompt_configurations()
#configuations = con2.prompt_configurations()
configuations = con2.prompt_configurations()
print('Your configurations:')
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