:go-pos method in pepper-interface.l does not work

[kochigami]

:go-pos publishes /cmd_pose topic, and :go-velocity publishes /cmd_vel topic.
The results of rostopic info /cmd_pose or /cmd_vel are as follows.

rostopic info /cmd_pose
Type: geometry_msgs/Pose2D

Publishers: 
 * /pepper_1444291883382908575 (http://133.11.216.136:54125/) (Note: This is from pepper-interface.l)

Subscribers: None

rostopic info /cmd_vel
Type: geometry_msgs/Twist

Publishers: 
 * /teleop_twist_joy (http://133.11.216.136:44116/)
 * /pepper_1444291883382908575 (http://133.11.216.136:54125/) (Note: This is from pepper-interface.l)

Subscribers: 
 * //pepper_robot (unknown address //pepper_robot)

I would like to know who is a subscriber of /cmd_pose. (Tomorrow I will look into it by using previous pepper environment.)
In addition, //pepper_robot as a subscriber for /cmd_vel topic seems weird. I think it comes from previous pepper.launch, but I don't know how to check it. If my guess is right, the process of /cmd_vel topic should also be modified.

The result of rosnode info //pepper_robot is as follows.

rosnode info //pepper_robot
--------------------------------------------------------------------------------
Node [//pepper_robot]
Publications: 
 * /pepper_robot/imu/base [sensor_msgs/Imu]
 * /pepper_robot/sonar/front [sensor_msgs/Range]
 * /pepper_robot/laser [sensor_msgs/LaserScan]
 * /pepper_robot/camera/ir/image_raw [sensor_msgs/Image]
 * /pepper_robot/camera/ir/camera_info [sensor_msgs/CameraInfo]
 * /pepper_robot/camera/depth/image_raw [sensor_msgs/Image]
 * /pepper_robot/camera/depth/camera_info [sensor_msgs/CameraInfo]
 * /pepper_robot/camera/bottom/camera_info [sensor_msgs/CameraInfo]
 * /pepper_robot/audio [naoqi_bridge_msgs/AudioBuffer]
 * /diagnostics [diagnostic_msgs/DiagnosticArray]
 * /joint_states [sensor_msgs/JointState]
 * /pepper_robot/camera/front/camera_info [sensor_msgs/CameraInfo]
 * /pepper_robot/sonar/back [sensor_msgs/Range]
 * /rosout [rosgraph_msgs/Log]
 * /pepper_robot/camera/bottom/image_raw [sensor_msgs/Image]
 * /tf [tf2_msgs/TFMessage]
 * /pepper_robot/camera/front/image_raw [sensor_msgs/Image]
 * /pepper_robot/imu/torso [sensor_msgs/Imu]
 * /pepper_robot/info [naoqi_bridge_msgs/StringStamped]

Subscriptions: 
 * /move_base_simple/goal [unknown type]
 * /joint_angles [unknown type]
 * /cmd_vel [geometry_msgs/Twist]

Services: 
 * /pepper_robot/get_loggers
 * /pepper_robot/set_logger_level
 * /naoqi_driver/get_robot_config

cannot contact [//pepper_robot]: unknown node

[kochigami]

I couldn't understand if it relates to this problem, but I found some topics in /pepper_robot/pose/pose_controller and pepper_robot/pose/pose_manager have unknown message type.
Tomorrow I will look into which is used for which.

Mainly, /pose_controller and manager publish and subscribe /joint_trajectory/cancel, goal, status, feedback and result topic.
pose_controller has /joint_stiffness and /joint_angles_action as unknown message type.
pose_manager has /body_pose as unknown message type.

pose_controller
publishers:
(all of them have /status, feedback, result)
/joint_trajectory
/joint_angles_action
/body_pose_naoqi
/joint_stiffness_trajectory

subscribers:
/joint_stiffness
/joint_angles
(below all of them have /cancel,goal)
/joint_angles_action
/joint_trajectory
/body_pose_naoqi
/joint_stiffness_trajectory

pose_manager
publishers:
/body_pose/feedback, status, result
/joint_trajectory/cancel, goal

subscribers:
/body_pose/goal, cancel
/joint_trajectory/status, result, feedback

[kochigami]

The results of trying previous pepper ROS environment:
launch wheel.launch in nao_apps in addition to jsk_pepper_startup.launch and peppereus
-> :go-pos :go-velocity succeeded.
-> The subscriber should be /nao_wheel for /cmd_vel and /cmd_pose topic.
-> I have to learn where & how to add wheel.launch and how to delete a weird //pepper_robot node.

About /pose_controller and /pose_manager:
I compared all topics (publisher, subscriber, message type) by rostopic info ...
-> In previous and new ROS environment, the types of topics and message types are same. In addition, there are no topic which has unknown message type. (It was my misunderstanding.)
-> In previous environment, /joint_trajectory, /joint_stiffness and /joint_angles are mainly used.
-> New environment lacks subscriber for /joint_stiffness (it should be /naoqi_joint_states).
-> I have to learn which code has a subscriber of /naoqi_joint_states and the function of all nodes.

[kochigami]

About /pose_controller and /pose_manager:
In naoqi_joint_states.py (old version), node is declared as naoqi_joint_states. This publishes /joint_stiffness, odom, imu and joint_states.

On the other hand, naoqi_driver.cpp (new version) does not publish it. Instead, it publishes different topics from those of naoqi_joint_states.py. It publishes basic, camera, info, joint_states, log and sonar related topics.

I also have to consider the role of /joint_stiffness topic, understand the new naoqi_driver.cpp.

[kochigami]

1. naoqi_pose package does not relate to :go-pos method.
2. /joint_stiffness is not used in pepper-interface.l, so there is no problem. (ALMotion setStiffnesses and stiffnessInterpolation are forbidden for Pepper's lower part, so it is inconvenient.)
3. For :go-pos method, just wheel.launch is required.

pose_controller.py:
simple action servers:
/joint_trajectory: ALMotion angleInterpolation(jointnames, angles, time) (pepper-interface.l uses it)
/joint_stiffness_trajectory: ALMotion stiffnessInterpolation(jointnames, stiffness, time, ) (pepper-interface.l uses it)
/joint_angles_action: ALMotion angleInterpolationWithSpeed(jointnames, angles. speed)
/body_pose_naoqi: ALRobotPosture move to predefined pose

subscribe topics:
/joint_stiffness: ALMotion setStiffnesses(list(names), list(efforts))
/joint_angles: ALMotion set/changeAngles(list(names), list(angles)) (pepper-interface.l uses it)

services:
/body_stiffness/enable: ALMotion stiffnessInterpolation
/body_stiffness/disable: same
/wakeup: ALMotion wakeup (pepper-interface.l uses it)
/rest: same (pepper-interface.l uses it)
/life/enable: ALAutonomousLife
/life/disable: same

[kochigami]

about //pepper_robot node:
It is from new naoqi_driver.launch, and @furushchev kindly helped me (pull-requests for naoqi_bridge and pepper_robot)( prevent calling ros::init without name; use node_name command option instead of namespace#44) ( [pepper_bringup/launch/pepper_full.launch] use node_name argument instead of namespace#11)
Actually, /cmd_vel topic appeared, but /cmd_pose was not there.

For :go-pos method, just wheel.launch is required.

Therefore it was not correct. I suggested /cmd_pose is also included in naoqi_driver.launch, because it was included in nao_walker.py. (naoqi_driver issue)( suggestion: adding /cmd_pose topic to naoqi_driver.launch#45 )

[k-okada]

Sorry for late response, The changes in naoqi_driver is to use c++ version if ROS-naoqi bridge instead of Python for efficiency. And that c++ bridge is not provide all fictionally as they provided in Python version.
So possible direction is to add required features to c++ version as ros-naoqi/naoqi_driver#43

So if we look into move/navigation related code at
https://github.com/ros-naoqi/naoqi_driver/blob/master/src/naoqi_driver.cpp#L788
And
https://github.com/ros-naoqi/naoqi_driver/blob/master/src/subscribers/moveto.cpp
It seems they provide move_base_simple/goal as pr2 robot does, so changing pepper-interface.l to send message to that message may the solution.

[k-okada]

please check if 0d717b8 of #452 works

[kochigami]

Thank you very much for telling me, and I am very sorry because I'm like a turtle to understand what I should do and how programs work. (I also have to be sorry for turtles.)
It worked well, :go-pos succedded.