[SOLVED]Kinectv2 not working with real ur5 but does in simulation
Hi everyone,
I am using the UR5 with a own designed gripper as end-effector and KinectV2 to 'map' the environment so that the UR5 will avoid collisions with a human it might be working with. I have run some tests:
- Simulated UR5 and simulated Kinect in gazebo. I can see the 'octomap' getting added to the planning scene in moveit.rviz and the UR5 avoids collisions with object placed in Gazebo.
- Simulated UR5 in Gazebo and real Kinect, same as above.
- Real UR5 and real Kinect. I cannot see the planning scene being added in moveit.rviz.
I get a warning from moveit that says:
[ WARN] [1516619887.119811534]: The complete state of the robot is not yet known. Missing driver_arm_joint
Where driver_arm_joint
is the joint that controls the gripper state.
My suggestion is that moveit cannot use the planning scene it is creating because the complete state of the robot is not yet known.
UPDATE: My gripper also becomes uncontrollable when launching the real UR5 and Kinectv2.
FINAL UPDATE: First I remapped the topic /joint_states (published by ur_modern_driver) to /joint_states_ur5. 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.
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.position)
data.effort = self.appendT(data.effort, self.position)
self.pub.publish(data)
if __name__ == '__main__':
ur5_r = ur5_republisher()
while not rospy.is_shutdown():
# self.cycle()
rospy.spin()
My question is: Is there a way to initialize the endeffector group such that you define a start state? Currently this is the content of the srdf:
<?xml version="1.0" ?>
<!--This does not replace URDF, and is not an extension of URDF.
This is a format for representing semantic information about the robot structure.
A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
-->
<robot name="ur5">
<!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
<!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
<!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
<!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents ...
re: your edit: take a look at the
source_list
parameter of the wiki/joint_state_publisher.It would also be nice if you could post your approach as an answer, and then accept your own answer.
That way it's a bit more obvious where the question ends and the answer starts.
@gvdhoorn Thank you for reminding me of the source_list param. I gave an answer to my own question like you asked. Altough I can't accept my own answer as correct because I don't have enough 'karma'. Would be nice if you could do that, if you agree.