diff --git a/CMakeLists.txt b/CMakeLists.txt index eed9c9a..83f375b 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -59,6 +59,7 @@ catkin_install_python(PROGRAMS scripts/following_marker.py scripts/follow_and_pump.py scripts/simple_gui.py + scripts/slider_600.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/config/mycobot_600.rviz b/config/mycobot_600.rviz index d3a3a19..145b7d8 100644 --- a/config/mycobot_600.rviz +++ b/config/mycobot_600.rviz @@ -163,7 +163,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 2.58317447 + Distance: 2.0593462 Enable Stereo Rendering: Stereo Eye Separation: 0.0599999987 Stereo Focal Distance: 1 @@ -178,10 +178,10 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.00999999978 - Pitch: 0.440398335 + Pitch: 0.660398126 Target Frame: Value: Orbit (rviz) - Yaw: 0.430389732 + Yaw: 3.10539246 Saved: ~ Window Geometry: Displays: diff --git a/scripts/slider_600.py b/scripts/slider_600.py new file mode 100755 index 0000000..1125824 --- /dev/null +++ b/scripts/slider_600.py @@ -0,0 +1,290 @@ +#!/usr/bin/python + +from socket import * +import math +import sys +import time +from multiprocessing import Lock + +import rospy +from sensor_msgs.msg import JointState + +global mc +mutex = Lock() + + +class ElephantRobot(object): + def __init__(self,host,port): + # setup connection + self.BUFFSIZE = 2048 + self.ADDR = (host, port) + self.tcp_client = socket(AF_INET, SOCK_STREAM) + + def start_client(self): + try: + self.tcp_client.connect(self.ADDR) + return '' + except error, e: + return e + + def stop_client(self): + self.tcp_client.close() + + def send_command(self,command): + with mutex: + self.tcp_client.send(command.encode()) + recv_data = self.tcp_client.recv(self.BUFFSIZE).decode() + res_str = str(recv_data) + print 'recv = ' + res_str + res_arr = res_str.split(":") + if len(res_arr) == 2: + return res_arr[1] + else: + return '' + + def string_to_coords(self, data): + data = data.replace('[','') + data = data.replace(']','') + data_arr = data.split(",") + if len(data_arr) == 6: + try: + coords_1 = float(data_arr[0]) + coords_2 = float(data_arr[1]) + coords_3 = float(data_arr[2]) + coords_4 = float(data_arr[3]) + coords_5 = float(data_arr[4]) + coords_6 = float(data_arr[5]) + coords = [coords_1, coords_2, coords_3, + coords_4, coords_5, coords_6] + return coords + except: + return invalid_coords() + return invalid_coords() + + def string_to_double(self, data): + try: + val = float(data) + return val + except: + return -9999.99 + + def string_to_int(self, data): + try: + val = int(data) + return val + except: + return -9999 + + def invalid_coords(self): + coords = [-1, -2, -3, -4, -1, -1] + return coords + + def get_angles(self): + command = 'get_angles()\n' + res = self.send_command(command) + return self.string_to_coords(res) + + def get_coords(self): + command = 'get_coords()\n' + res = self.send_command(command) + return self.string_to_coords(res) + + def get_speed(self): + command = 'get_speed()\n' + res = self.send_command(command) + return self.string_to_double(res) + + def power_on(self): + command = 'power_on()\n' + res = self.send_command(command) + return True + + def power_off(self): + command = 'power_off()\n' + res = self.send_command(command) + return True + + def check_running(self): + command = 'check_running()\n' + res = self.send_command(command) + return res == '1' + + def state_check(self): + command = 'state_check()\n' + res = self.send_command(command) + return res == '1' + + def program_open(self, file_path): + command = 'program_open(' + file_path + ")\n" + res = self.send_command(command) + return self.string_to_int(res) + + def program_run(self, start_line): + command = 'program_run(' + str(start_line) + ")\n" + res = self.send_command(command) + return self.string_to_int(res) + + def read_next_error(self): + command = 'read_next_error()\n' + res = self.send_command(command) + return res + + def write_coords(self,coords,speed): + command = "set_coords(" + for item in coords: + command += str(item) + ',' + command += str(speed) + ')\n' + self.send_command(command) + + def write_coord(self, axis, value, speed): + coords = self.get_coords() + if coords != self.invalid_coords(): + coords[axis] = value + self.write_coords(coords,speed) + + def write_angles(self,angles,speed): + command = "set_angles(" + for item in angles: + command += str(item) + ',' + command += str(speed) + ')\n' + self.send_command(command) + + def write_angle(self, joint, value, speed): + angles = self.get_angles() + if angles != self.invalid_coords(): + angles[joint] = value + self.write_angles(angles,speed) + + def set_speed(self, percentage): + command = "set_speed(" + str(percentage) + ")\n" + self.send_command(command) + + def set_carte_torque_limit(self, axis_str, value): + command = "set_torque_limit(" + axis_str + "," + str(value) + ")\n" + self.send_command(command) + + def set_upside_down(self, up_down): + up = "1" + if up_down: + up = "0" + command = "set_upside_down(" + up + ")\n" + self.send_command(command) + + def set_payload(self, payload): + command = "set_speed(" + str(payload) + ")\n" + self.send_command(command) + + def state_on(self): + command = "state_on()\n" + self.send_command(command) + + def state_off(self): + command = "state_off()\n" + self.send_command(command) + + def task_stop(self): + command = "task_stop()\n" + self.send_command(command) + + def jog_angle(self, joint_str, direction, speed): + command = "jog_angle(" + joint_str + "," + str(direction) + "," + str(speed) + ")\n" + self.send_command(command) + + def jog_coord(self, axis_str, direction, speed): + command = "jog_coord(" + axis_str + "," + str(direction) + "," + str(speed) + ")\n" + self.send_command(command) + + def get_digital_in(self, pin_number): + command = "get_digital_in(" + str(pin_number) + ")\n" + self.send_command(command) + + def get_digital_out(self, pin_number): + command = "get_digital_out(" + str(pin_number) + ")\n" + self.send_command(command) + + def set_digital_out(self, pin_number, pin_signal): + command = "set_digital_out(" + str(pin_number) + "," + str(pin_signal) + ")\n" + self.send_command(command) + + def set_analog_out(self, pin_number, pin_signal): + command = "set_analog_out(" + str(pin_number) + "," + str(pin_signal) + ")\n" + self.send_command(command) + + def get_acceleration(self): + command = "get_acceleration()\n" + res = self.send_command(command) + return self.string_to_int(res) + + def set_acceleration(self, acceleration): + command = "set_acceleration(" + str(acceleration) + ")\n" + self.send_command(command) + + def command_wait_done(self): + command = "wait_command_done()\n" + self.send_command(command) + + def wait(self, seconds): + command = "wait(" + str(seconds) + ")\n" + self.send_command(command) + + def assign_variable(self, var_name, var_value): + command = 'assign_variable("' + str(var_name) + '",' + str(var_value) + ')\n' + self.send_command(command) + + def get_variable(self, var_name): + command = 'get_variable("' + str(var_name) + '")\n' + return self.send_command(command) + + + +old_list = [] + +def callback(data): + global old_list + #rospy.loginfo(rospy.get_caller_id() + "%s", data.position) + print('position', data.position) + data_list = [] + for index, value in enumerate(data.position): + value = value * 180 / math.pi + data_list.append(value) + print('data', data_list) + + if not old_list: + old_list = data_list + mc.write_angles(data_list, 1999) + elif old_list != data_list: + old_list = data_list + if (mc.check_running()): + mc.task_stop() + time.sleep(0.05) + + mc.write_angles(data_list, 1999) + + +def listener(): + global mc + rospy.init_node('control_slider', anonymous=True) + + ip = rospy.get_param('~ip', '192.168.10.169') + print(ip) + mc = ElephantRobot(ip, 5001) + # START CLIENT + res = mc.start_client() + if res != '': + print res + sys.exit(1) + print ep.wait(5) + print mc.get_angles() + print mc.get_coords() + mc.set_speed(30) + print mc.get_speed() + + rospy.Subscriber("joint_states", JointState, callback) + + # spin() simply keeps python from exiting until this node is stopped + print('sping ...') + rospy.spin() + + +if __name__ == '__main__': + listener() diff --git a/urdf/600_urdf/mycobot_600_urdf.urdf b/urdf/600_urdf/mycobot_600_urdf.urdf index 5b37387..734fc9f 100755 --- a/urdf/600_urdf/mycobot_600_urdf.urdf +++ b/urdf/600_urdf/mycobot_600_urdf.urdf @@ -142,16 +142,16 @@ - + - + - + @@ -159,11 +159,11 @@ - + - +