how to connect robot model to robot without use moveit

[ZouJennie]

By using the rostopic information of /qt_nuitrack_app, I built a program that displays the TF of skeletons. As we know each skeleton has 25 joints, and the TFs of all joints are connected to the same parent frame, I call it /nuitrack_frame. In order to visualize all the informations well on rviz, I want to import the urdf file of qtrobot and connect my /nuitrack_frame to the TF of Camera_link of the robot model. So i wrote a launch file which connect these two TF by using static_transform_publisher:

<launch>
    <arg name="model" />
    <arg name="gui" default="False" />
    <param name="robot_description" textfile="$(find qt_nuitrack_tool)/urdf/qtrobot.urdf" />
    <param name="use_gui" value="$(arg gui)"/>
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
    <node pkg="qt_nuitrack_tool" type="tf_skeletons.py" name="skeletons" />
    <node pkg="tf" type="static_transform_publisher" name="camera_base_link"
      args="0 0 0 3.14 3.14 0 Camera_link /nuitrack_frame 100" />
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find qt_nuitrack_tool)/launch/moveit.rviz" />
</launch>

But i saw in Rviz, the robot model is not really connect to the robot. Because when i turn around the head by using rostopic pub /qt_robot/head_position/command, the TF of head did not turn as in the real. So I don't know what need i do to connect the robot model to the robot without using moveit. Because with Moveit, we can't move the head actuelly.

Thank you for your attention.

[apaikan]

Hi @ZouJennie Did you follow the motors_moveit tutorial first?

As it is described in the tutorial, you do not need to load the robot URDF, it is already published by qtrobot via /tf.
From what I see, you have MISSED the main step in the tutorial? Change motor launcher autostart script to run in advanced mode!

When you launch the qt_motor in advance mode you wont see /qt_robot/head_position/command anymore!!!! instead you have to use the ROS position_controllers/JointTrajectoryController to move the head or arms of the robot!

qtrobot@QTXXX: rostopic type  /qt_robot/left_arm_controller/follow_joint_trajectory/goal
control_msgs/FollowJointTrajectoryActionGoal

[apaikan]

I also recommend that you first setup QTrobot in the advanced mode and check all the published topics. If you simply open RViz, and add the robot description/TF, you will see all the joints positions are updated correctly

[ZouJennie]

But when i changed to advanced mode, I lost all the topic of nuitrack. In the autostart_screen.sh there is start_qt_nuitrack_app.sh, and I didn't change anything except change qt_motor_advanced in the start_qt_motor.sh. That are all the rostopic i receved when i change to advanced mode:

/add_file_to_robot
/camera/color/image_raw
/client_count
/connected_clients
/find_object/objects
/interactive_interface_notes
/kid_tablet_images
/objectsStamped
/qt_robot/audio/play
/qt_robot/behavior/talkAudio
/qt_robot/behavior/talkText
/qt_robot/emotion/show
/qt_robot/gesture/play
/qt_robot/head_controller/command
/qt_robot/head_controller/follow_joint_trajectory/cancel
/qt_robot/head_controller/follow_joint_trajectory/feedback
/qt_robot/head_controller/follow_joint_trajectory/goal
/qt_robot/head_controller/follow_joint_trajectory/result
/qt_robot/head_controller/follow_joint_trajectory/status
/qt_robot/head_controller/state
/qt_robot/joints/state
/qt_robot/left_arm_controller/command
/qt_robot/left_arm_controller/follow_joint_trajectory/cancel
/qt_robot/left_arm_controller/follow_joint_trajectory/feedback
/qt_robot/left_arm_controller/follow_joint_trajectory/goal
/qt_robot/left_arm_controller/follow_joint_trajectory/result
/qt_robot/left_arm_controller/follow_joint_trajectory/status
/qt_robot/left_arm_controller/state
/qt_robot/motors/states
/qt_robot/right_arm_controller/command
/qt_robot/right_arm_controller/follow_joint_trajectory/cancel
/qt_robot/right_arm_controller/follow_joint_trajectory/feedback
/qt_robot/right_arm_controller/follow_joint_trajectory/goal
/qt_robot/right_arm_controller/follow_joint_trajectory/result
/qt_robot/right_arm_controller/follow_joint_trajectory/status
/qt_robot/right_arm_controller/state
/qt_robot/speech/say
/robAPL_input_events
/robAPL_input_game
/robot_add_file_request
/robot_add_file_response
/rosout
/rosout_agg
/tf
/tf_static

Another problem is my robot can not connect to QTPC every time after reboot, i need to turn off and wait for long time, then when i reopen the robot, i can connect to QTPC.

[ZouJennie]

I saw that in advanced mode, when i play a gesture the /TF doesn't show the movement. I think that is what you said during last meeting, gesture is a simple mouvement which doesn't passe by motor. But I don't know how to command the head in advanced mode, the rostopic position_controllers/JointTrajectoryController need too much information. Can you give me an example?

[apaikan]

Hi @ZouJennie Regarding gesture controller and Joint state publisher When playing a gesture with the current implementation, the QTrobot gesture controller suspends the Joint state publisher thread until the gesture being played. That's why you dont see any update in /TF. We can update the gesture controller package to enable this feature.

Regarding the position_controllers/JointTrajectoryController if you want to move it from GUI you can use Joint trajectory controller plugin from rqt. see an example bellow:

There are also plenty example from ROS to how to send command using ROS joint trajectory controller. For example see the link bellow:
http://library.isr.ist.utl.pt/docs/roswiki/pr2_controllers(2f)Tutorials(2f)Moving(20)the(20)arm(20)using(20)the(20)Joint(20)Trajectory(20)Action.html

P.S. we will update our tutorial to add more example

[ZouJennie]

Thank you very much

[codionysus]

I saw that in advanced mode, when i play a gesture the /TF doesn't show the movement. I think that is what you said during last meeting, gesture is a simple mouvement which doesn't passe by motor. But I don't know how to command the head in advanced mode, the rostopic position_controllers/JointTrajectoryController need too much information. Can you give me an example?

Hi @ZouJennie

To be able to see movement of played gestures you need edit qtrobot-controller-extra.yaml config file on QTRP and add this two lines:

gesture:
      adjust_mainloop_frequency: false

This is how qtrobot-controller-extra.yaml file should look like. Make sure that to indent the lines like this (qt_robot -> gesture):

qt_robot:
  joints_state:
    publish_rate: 30

  gesture:
    adjust_mainloop_frequency: false

File is located in /opt/ros/kinetic/share/qt_motor/config/qtrobot-controller-extra.yaml After that reboot the QTrobot.