From: Erik Andresen <erik@vontaene.de>
Date: Sun, 18 Oct 2015 09:48:37 +0000 (+0200)
Subject: publish joint messages, added gripper 35978
X-Git-Url: https://defiant.homedns.org/gitweb/?a=commitdiff_plain;h=2c6e57243361122f521f6a1aaa1e443af88a261a;p=arm_ros_conn.git

publish joint messages, added gripper 35978
---

diff --git a/launch/arm.launch b/launch/arm.launch
index 3be44ff..ce1ed91 100644
--- a/launch/arm.launch
+++ b/launch/arm.launch
@@ -2,6 +2,7 @@
 <launch>
 	<param name="robot_description" textfile="$(find arm)/urdf/arm.urdf" />
 
-	<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
-	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen"/>
+
+	<node pkg="arm" type="arm_ros_conn.py" name="arm_ros_conn" output="screen" respawn="true"/>
 </launch>
diff --git a/meshes/35978.stl b/meshes/35978.stl
new file mode 100644
index 0000000..fd256fd
Binary files /dev/null and b/meshes/35978.stl differ
diff --git a/scripts/arm_ros_conn.py b/scripts/arm_ros_conn.py
index 9ca59f2..c4ab470 100755
--- a/scripts/arm_ros_conn.py
+++ b/scripts/arm_ros_conn.py
@@ -2,14 +2,35 @@
 # -*- coding: iso-8859-15 -*-
 
 import rospy
+import arm
+from sensor_msgs.msg import JointState
 
 class ARMRosConn():
 	def __init__(self):
 		rospy.init_node('arm')
 
-		rate = rospy.Rate(10)
+		arm.switch(0)
+		arm.switch(2)
+		arm.set_hall_mode(3, 0)
+		arm.set_hall_mode(5, 0)
+		arm.set_tolerance(3, 0)
+		arm.set_tolerance(5, 0)
+
+		self.pub_joint_states = rospy.Publisher("joint_states", JointState, queue_size=16)
+		self.run()
+
+	def run(self):
+		rate = rospy.Rate(20)
 		while not rospy.is_shutdown():
+			self.publish_joint_states()
 			rate.sleep()
+	
+	def publish_joint_states(self):
+		joint_state = JointState()
+		joint_state.header.stamp = rospy.Time.now()
+		joint_state.name = ["base_to_link1", "link_1_2_joint", "link_2_3_joint", "gripper_joint_1", "gripper_joint_2", "left_gripper_joint", "right_gripper_joint"]
+		joint_state.position = [arm.get_angle(0), arm.get_angle(1), arm.get_angle(2), arm.get_angle(3), arm.get_angle(4), arm.get_angle(5)/2, arm.get_angle(5)/2]
+		self.pub_joint_states.publish(joint_state)
 
 
 if __name__ == '__main__':
diff --git a/urdf/arm.urdf b/urdf/arm.urdf
index 24587c0..62d8e8a 100644
--- a/urdf/arm.urdf
+++ b/urdf/arm.urdf
@@ -42,9 +42,9 @@
 	<link name="gripper_pole1">
 		<visual>
 			<geometry>
-				<cylinder radius="0.04" length="0.055"/>
+				<cylinder radius="0.045" length="0.03"/>
 			</geometry>
-			<origin xyz="0.0275 0 0" rpy="0 1.57075 0"/>
+			<origin xyz="0.015 0 0" rpy="0 1.57075 0"/>
 			<material name="black">
 				<color rgba="0 0 0 1"/>
 			</material>
@@ -54,26 +54,29 @@
 	<link name="gripper_pole2">
 		<visual>
 			<geometry>
-				<cylinder radius="0.0" length="0.0"/>
+				<cylinder radius="0.03" length="0.015"/>
 			</geometry>
-			<origin xyz="0.0 0 0" rpy="0 0 0"/>
+			<origin xyz="0.0 0 0" rpy="1.57075 0 0"/>
+			<material name="red">
+				<color rgba="1 0 0 1"/>
+			</material>
 		</visual>
 	</link>
 
 	<link name="left_gripper">
 		<visual>
-			<origin rpy="0.0 0 0" xyz="0 0 0"/>
+			<origin xyz="0.025 0 0" rpy="-1.57075 3.1415 0"/>
 			<geometry>
-				<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
+				<mesh filename="package://arm_ros_conn/meshes/35978.stl" scale="0.001 0.001 0.001"/>
 			</geometry>
 		</visual>
 	</link>
 
 	<link name="right_gripper">
 		<visual>
-			<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
+			<origin xyz="0.025 0 0" rpy="1.57075 3.1415 0"/>
 			<geometry>
-				<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
+				<mesh filename="package://arm_ros_conn/meshes/35978.stl" scale="0.001 0.001 0.001"/>
 			</geometry>
 		</visual>
 	</link>
@@ -113,23 +116,23 @@
 		<parent link="gripper_pole1"/>
 		<child link="gripper_pole2"/>
 		<axis xyz="0 1 0"/>
-		<origin xyz="0.055 0.0 0.0" rpy="0 0 0"/>
+		<origin xyz="0.06 0.0 0.0" rpy="0 0 0"/>
 		<limit effort="1000.0" lower="-1.5707963267948966" upper="1.5707963267948966" velocity="1.0"/>
 	</joint>
 
-	<joint name="right_gripper_joint" type="revolute">
-		<parent link="gripper_pole2"/>
-		<child link="right_gripper"/>
-		<axis xyz="0 0 -1"/>
-		<origin xyz="0 -0.01 0" rpy="0 0 0"/>
-		<limit effort="1000.0" lower="0.0" upper="0.548" velocity="1.0"/>
-	</joint>
-
 	<joint name="left_gripper_joint" type="revolute">
 		<parent link="gripper_pole2"/>
 		<child link="left_gripper"/>
 		<axis xyz="0 0 1"/>
-		<origin xyz="0 0.01 0" rpy="0 0 0" />
-		<limit effort="1000.0" lower="0.0" upper="0.548" velocity="1.0"/>
+		<origin xyz="0.02 0.01 0" rpy="0 0 0" />
+		<limit effort="1000.0" lower="0.0" upper="0.35" velocity="1.0"/>
+	</joint>
+
+	<joint name="right_gripper_joint" type="revolute">
+		<parent link="gripper_pole2"/>
+		<child link="right_gripper"/>
+		<axis xyz="0 0 -1"/>
+		<origin xyz="0.02 -0.01 0" rpy="0 0 0"/>
+		<limit effort="1000.0" lower="0.0" upper="0.35" velocity="1.0"/>
 	</joint>
 </robot>