[SOLVED]Kinectv2 not working with real ur5 but does in simulation

edit

I was able to find the problem and solve it: I found out that the error I was having came from line 1188 from this header, http://docs.ros.org/jade/api/moveit_r...

Then I realized that the ur_modern_driver is keeping the /joint_states topic occupied so I remapped the /joint_states topic to /joint_states_ur5 by adding this line to the spawning of the ur_driver node(ur_modern_driver/launch/ur_common.launch):

<node name="ur_driver" pkg="ur_modern_driver" type="ur_driver" output="screen">
....
    <remap from="joint_states" to="joint_states_ur5"/> <!-- THIS -->
....
</node>

Then I wrote a node that subscribes to the /joint_states_ur5 topic and add a sensors_msgs/JointState message to the tuple of the gotten published by the ur_driver. Then I republish the message under the topic /joint_states. The python node is shown below(FIRST NODE).

NOTE: For the updated python node (that includes setting the self.position based on a boolean rosparam look for the SECOND NODE).

FIRST NODE:

import math
import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import Bool


class ur5_republisher():
    def __init__(self):
        rospy.init_node('Cmd_Scaler')
        self.pub = rospy.Publisher('/joint_states', JointState, queue_size=10)
        rospy.Subscriber("/joint_states_ur5", JointState, self.joint_state_callback)

        self.name = 'driver_arm_joint'
        self.position = 0
        self.velocity = 0
        self.effort = 0

    def appendT(self,t,x):
    l = list(t)
    l.append(x)
    return tuple(l)

    def joint_state_callback(self, data):
        data.name.append(self.name)
    data.position = self.appendT(data.position, self.position)
    data.velocity = self.appendT(data.velocity, self.velocity)
    data.effort = self.appendT(data.effort, self.effort)
       self.pub.publish(data)

if __name__ == '__main__':
    ur5_r = ur5_republisher()
    while not rospy.is_shutdown():
    # self.cycle()
    rospy.spin()

SECOND NODE:

import math
import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import Bool

class ur5_republisher():
    def __init__(self):
        rospy.init_node('Cmd_Scaler')
        self.pub = rospy.Publisher('/joint_states', JointState, queue_size=10)
        rospy.Subscriber("/joint_states_ur5", JointState, self.joint_state_callback)
        self.name = 'driver_arm_joint'
        self.position = self.checkGripperState()
        self.velocity = 0
        self.effort = 0
    def appendT(self,t,x):
        l = list(t)
        l.append(x)
        return tuple(l)
    def checkGripperState(self):
        try:
            gripper_state = rospy.get_param('/bool_gripperOpen_param')
            if(gripper_state):
                gripper_position = 0.25
                return gripper_position
            if not(gripper_state):
                gripper_position = 0.0
                return gripper_position
        except KeyError:
            gripper_state = 0.0
            print("rosparam, bool_gripperOpen_param not set")
            return gripper_state
    def joint_state_callback(self, data):
        data.name.append(self.name)
        data.position = self.appendT(data.position, self.position)
        data.velocity = self.appendT(data.velocity, self.velocity)
        data.effort = self.appendT(data.effort, self.effort)
        self.pub.publish(data)

if __name__ == '__main__':
    ur5_r = ur5_republisher()
    while not rospy.is_shutdown():
        # self.cycle()
        rospy.spin()