publish joint messages, added gripper 35978

diff --git a/launch/arm.launch b/launch/arm.launch
index 3be44ff48c36bb62c5819acea32bec4a1a8695dd..ce1ed916c21120abef1442246a08157c264819c4 100644 (file)
--- 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 (file)
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 9ca59f268c147b55cec9168b1b6ef49a448126c7..c4ab470bf71d21b4f110f36adba824f1b3db4fca 100755 (executable)
--- 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 24587c0e1d548193c942cc99e57f03b5788a9509..62d8e8a5b14295fad490f5c4a17eacadf52df7ae 100644 (file)
--- 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>