amedwards · futureneer · Jul 15, 2014 · Jul 15, 2014 · Jul 15, 2014 · Jul 16, 2014
diff --git a/ur5_robotiq_description/src/urdf/ur5_robotiq_joint_limited.urdf.xacro b/ur5_robotiq_description/src/urdf/ur5_robotiq_joint_limited.urdf.xacro
@@ -6,7 +6,7 @@
   <xacro:robot></xacro:robot>
 
 
-  <xacro:include filename="$(find ur5_robotiq_description)/src/urdf/robotiq.urdf.xacro" />
+  <!-- <xacro:include filename="$(find ur5_robotiq_description)/src/urdf/robotiq.urdf.xacro" /> -->
   <xacro:robotiq_c_model root_link="ee_fixed_link" show_underactuated="false" gravity="false"></xacro:robotiq_c_model>
 
   <!--

diff --git a/ur_description/urdf/ur5.urdf.xacro b/ur_description/urdf/ur5.urdf.xacro
@@ -56,7 +56,7 @@ center_of_mass = [ [0, -0.02561, 0.00193], [0.2125, 0, 0.11336], [0.11993, 0.0,
       <geometry>
 		    <mesh filename="package://ur_description/meshes/ur5/visual/Base.dae" />
       </geometry>
-      <origin xyz="0.0 0.0 0.003" rpy="0.0 0.0 ${3.0 * pi / 4.0}" />
+      <origin xyz="0.0 0.0 0.003" rpy="0.0 0.0 ${-pi/4.0}" />
     </visual>
 	<collision>
       <geometry>
@@ -73,7 +73,7 @@ center_of_mass = [ [0, -0.02561, 0.00193], [0.2125, 0, 0.11336], [0.11993, 0.0,
   <joint name="${prefix}shoulder_pan_joint" type="revolute">
     <parent link="${prefix}base_link" />
     <child link = "${prefix}shoulder_link" />
-    <origin xyz="0.0 0.0 ${shoulder_height}" rpy="0.0 0.0 0.0" />
+    <origin xyz="0.0 0.0 ${shoulder_height}" rpy="0.0 0.0 3.1415" />
     <axis xyz="0 0 1" />
     <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="150.0" velocity="${pi}"/>
     <dynamics damping="0.4" friction="0.0"/>
@@ -247,8 +247,8 @@ center_of_mass = [ [0, -0.02561, 0.00193], [0.2125, 0, 0.11336], [0.11993, 0.0,
   </joint>
 
   <link name="${prefix}ee_link" />
-
-  <xacro:ur5_arm_transmission prefix="${prefix}" />
+  <!-- KGUERIN CHANGED TO FIX ERROR IN URDF_PARSER_PY  -->
+  <!-- <xacro:ur5_arm_transmission prefix="${prefix}" /> -->
   <xacro:ur5_arm_gazebo prefix="${prefix}" />
 
   </xacro:macro>

diff --git a/ur_driver/.gitignore b/ur_driver/.gitignore
@@ -0,0 +1 @@
+*.ini
diff --git a/ur_driver/CMakeLists.txt b/ur_driver/CMakeLists.txt
@@ -4,7 +4,12 @@ project(ur_driver)
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
-find_package(catkin REQUIRED)
+find_package(catkin REQUIRED COMPONENTS
+  message_generation
+  geometry_msgs
+  rospy
+  rqt_gui
+  rqt_gui_py)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -26,18 +31,24 @@ catkin_python_setup()
 #   Message2.msg
 # )
 
-## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
+# Generate services in the 'srv' folder
+add_service_files(
+  FILES
+  FreeDrive.srv
+  GetTcpPose.srv
+  ServoToPose.srv
+  ServoEnable.srv
+  MoveToPose.srv
+  Home.srv
+  Stop.srv
+)
 
-## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   control_msgs#   sensor_msgs
-# )
+# Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+  geometry_msgs#   sensor_msgs
+)
 
 ###################################
 ## catkin specific configuration ##
@@ -47,7 +58,9 @@ catkin_python_setup()
 ## LIBRARIES: libraries you create in this project that dependent projects also need
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
-catkin_package()
+catkin_package(
+	CATKIN_DEPENDS message_runtime
+)
 
 
 ###########
@@ -56,7 +69,9 @@ catkin_package()
 
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
-# include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
+include_directories(
+	${catkin_INCLUDE_DIRS}
+)
 
 ## Declare a cpp library
 # add_library(ur_driver

diff --git a/ur_driver/freedrive_test.py b/ur_driver/freedrive_test.py
@@ -0,0 +1,38 @@
+#!/usr/bin/env python
+import time
+import roslib; roslib.load_manifest('ur_driver')
+import rospy
+import actionlib
+from control_msgs.msg import *
+from trajectory_msgs.msg import *
+from geometry_msgs.msg import *
+import PyKDL
+from ur_driver.srv import *
+import tf_conversions as tf_c
+
+if __name__ == '__main__':
+    current_pose = None
+
+    rospy.init_node("test_servo", anonymous=True, disable_signals=True)
+
+    print 'waiting for service'
+    rospy.wait_for_service('/ur_driver/free_drive')
+    print 'calling service with true'
+    try:
+        free_drive_service = rospy.ServiceProxy('/ur_driver/free_drive',free_drive)
+        result = free_drive_service(True)
+        print 'Service returned: ' + str(result.ack)
+    except rospy.ServiceException, e:
+        print e
+
+    rospy.sleep(5)
+
+    print 'waiting for service'
+    rospy.wait_for_service('/ur_driver/free_drive')
+    print 'calling service with false'
+    try:
+        free_drive_service = rospy.ServiceProxy('/ur_driver/free_drive',free_drive)
+        result = free_drive_service(False)
+        print 'Service returned: ' + str(result.ack)
+    except rospy.ServiceException, e:
+        print e
diff --git a/ur_driver/launch/servo_driver.launch b/ur_driver/launch/servo_driver.launch
@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<launch>
+  <!-- robot_ip: IP-address of the robot's socket-messaging server -->
+  <arg name="robot_ip" />
+
+  <!-- copy the specified IP address to be consistant with ROS-Industrial spec.
+  NOTE: The ip address is actually passed to the driver on the command line -->
+  <param name="/robot_ip_address" type="str" value="$(arg robot_ip)"/>
+
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+
+  <!-- driver -->
+  <node name="ur_driver" pkg="ur_driver" type="servo_driver.py" args="$(arg robot_ip)" output="screen" />
+
+</launch>
diff --git a/ur_driver/launch/test_common.launch b/ur_driver/launch/test_common.launch
@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<!--
+  Universal robot common bringup.  Starts ur driver node and robot state
+  publisher (translates joint positions to propper tfs).
+
+  NOTE: This launch file also starts a tf2 node, which is not typically
+  used
+
+  Usage:
+    ur_common.launch robot_ip:=<value>
+-->
+<launch>
+  <!-- robot_ip: IP-address of the robot's socket-messaging server -->
+  <arg name="robot_ip" />
+
+  <!-- copy the specified IP address to be consistant with ROS-Industrial spec.
+  NOTE: The ip address is actually passed to the driver on the command line -->
+  <param name="/robot_ip_address" type="str" value="$(arg robot_ip)"/>
+
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+
+  <!-- driver -->
+  <node name="ur_driver" pkg="ur_driver" type="driver.py" args="$(arg robot_ip)" output="screen" />
+
+  <!-- TF Buffer Server -->
+  <node pkg="tf2_ros" type="buffer_server" name="tf2_buffer_server">
+    <param name="buffer_size" value="120.0"/>
+  </node>
+
+</launch>
diff --git a/ur_driver/launch/test_servo_driver.launch b/ur_driver/launch/test_servo_driver.launch
@@ -0,0 +1,52 @@
+<?xml version="1.0"?>
+
+<launch>
+
+  <!-- <node name="rviz" pkg="rviz" type="rviz" args="-d $(find ur_driver)/ur.rviz" /> -->
+
+  <!-- <node name="ur_status_interface" pkg="ur_driver" type="ur_status_interface.py"/> -->
+  <!-- <node name="marker_teleop_interface" pkg="ur_driver" type="marker_teleop_interface.py"/> -->
+
+  <node name="target_frame_broadcaster" pkg="lcsr_tf_tools" type="interactive_transform_publisher" args="-.109 -.72 1.47 3.1415 0 -1.5707 /world /target_frame 10"/>
+
+  <!-- <node pkg="tf" type="static_transform_publisher" name="world_to_ur_base" args=" 0 0 0 0 0 0  /world /base_link 100"/> -->
+
+  <!-- <node pkg="tf" type="static_transform_publisher" name="world_to_base_link" args=" 0 0 1.4605 0 0 2.3565  /world /base_link 10"/> -->
+  <node pkg="tf" type="static_transform_publisher" name="world_to_base_link" args=" 0 0 1.4605 3.1415 0 -2.3565 /world /base_link 10"/>
+
+  <node pkg="tf" type="static_transform_publisher" name="ee_to_robotiq_c" args=" 0 0 0 0 0 0  /ee_link /robotiq_85_adapter_link 10"/>
+
+  <node pkg="tf" type="static_transform_publisher" name="ee_to_wrist_camera" args=" 0 0 -.045 0 0 0   /ee_link /camera_1_link 10"/>
+
+  <!-- robot_ip: IP-address of the robot's socket-messaging server -->
+  <arg name="robot_ip" />
+  <arg name="limited" default="false"/>
+
+  <!-- robot model -->
+  <include file="$(find ur_description)/launch/ur5_upload.launch">
+    <arg name="limited" value="$(arg limited)"/>
+  </include>
+
+  <!-- gripper model -->
+  <group ns="/ur5/gripper">
+    <arg name="model" default="$(find robotiq_c_model_visualization)/cfg/robotiq_c_model.urdf" />
+    <arg name="gui" default="False" />
+    <param name="robot_description" textfile="$(arg model)" />
+    <param name="use_gui" value="$(arg gui)"/>
+    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" >
+      <remap from="joint_states" to="/ur5/gripper/joint_states"/>
+    </node>
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher">
+      <remap from="joint_states" to="/ur5/gripper/joint_states"/>
+    </node>
+    <rosparam>
+      source_list: [/ur5/gripper/joint_states]
+    </rosparam>
+  </group>
+
+  <!-- ur common -->
+  <include file="$(find ur_driver)/launch/servo_driver.launch">
+    <arg name="robot_ip"   value="$(arg robot_ip)" />
+  </include>
+
+</launch>