Problems in controlling Pepper with ros-melodic

[souljaboy764]

I had initially posted this issue under pepper_dcm_robot (ros-naoqi/pepper_dcm_robot#6) but found that the issue was due to the naoqi_dcm_driver package so I'm posting the problem and one workaround that worked for me here.

ISSUE

I am trying to control Pepper from ubuntu 18.04 with ros-melodic. Since all packages weren't available with sudo apt install, installed whatever was available
sudo apt install ros-melodic-naoqi-bridge-msgs ros-melodic-naoqi-libqi ros-melodic-naoqi-driver ros-melodic-naoqi-libqicore
sudo apt install ros-melodic-pepper-meshes

I have cloned the rest of the packages into my catkin_ws

neo@zion:~$ ls catkin_ws/src/pepper_*
catkin_ws/src/pepper_dcm_robot:
pepper_dcm_bringup  README.rst

catkin_ws/src/pepper_moveit_config:
CHANGELOG.rst  CMakeLists.txt  config  launch  package.xml  README.rst  test  tuto

catkin_ws/src/pepper_robot:
pepper_bringup  pepper_description  pepper_robot  pepper_sensors_py

catkin_ws/src/pepper_virtual:
pepper_control  pepper_gazebo_plugin  README.md

neo@zion:~$ ls catkin_ws/src/naoqi_*
catkin_ws/src/naoqi_bridge:
LICENSE.txt  naoqi_bridge     naoqi_navigation  naoqi_sensors_py  README.md
naoqi_apps   naoqi_driver_py  naoqi_pose        naoqi_tools       tracks.yaml

catkin_ws/src/naoqi_dcm_driver:
CHANGELOG.rst  CMakeLists.txt  include  package.xml  README.rst  src

and installed whatever dependencies using

neo@zion:~/catkin_ws$ rosdep install --from-paths src --ignore-src -r -y

After source catkin_ws/devel/setp.bash I run the following in different terminals one after the other

To start up the robot:

neo@zion:~$ roslaunch pepper_dcm_bringup pepper_bringup.launch robot_ip:=192.168.100.186 network_interface:=enp7s0 

The output of this can be seen here: https://pastebin.com/3VenKwum

Once the robot has started up, I run:

neo@zion:~$ roslaunch naoqi_driver naoqi_driver.launch nao_ip:=192.168.100.186 network_interface:=enp7s0

Using this, I am able to see the images and pointcloud from the robot and also visualize the robot's pose accurately in rviz when I passively move the hands. The output of this command can be seen here: https://pastebin.com/n9S5vN5t

And finally, I run moveit using:

neo@zion:~$ roslaunch pepper_moveit_config moveit_planner.launch 

The output of this can be seen here: https://pastebin.com/utPV5yZY

I use the moveit interface to generate a random valid goal configuration (shown in orange) from the MotionPlanning interface and then I first plan it (as seen in the trail).

When I click on execute, nothing happens with the robot even though the following messages are displayed in the terminal

[ INFO] [1594216249.619841838]: Execution request received
[ INFO] [1594216253.127858948]: Controller pepper_dcm/RightArm_controller successfully finished
[ INFO] [1594216253.334119901]: Completed trajectory execution with status SUCCEEDED ...
[ INFO] [1594216253.334365776]: Execution completed: SUCCEEDED

There is nothing that gets printed in the terminal where pepper_dcm_bringup is running.

I try to publish the joint trajectories manually first using the topic /pepper_dcm/RightArm_controller/follow_joint_trajectory/goal which does not give any movement on the robot either.
I then try to publish the joint trajectories on the topic /pepper_dcm/RightArm_controller/command which does not give any movement on the robot as well.
There is nothing that gets printed in the terminal where pepper_dcm_bringup is running, even when manually publishing the trajectories on the topics.

I have tried this out with two different pepper robots and it does not work in either case.

The same commands when executed on a VM running ubuntu 16.04 with ros-kinetic seem to work with both robots moving to the specified location in the moveit interface.

WORKAROUND

While going through the motion.cpp file, I saw that only in the function writeJoints, the inputs were converted to qi::AnyValue before using it as an argument from motion_proxy_ as shown below.

naoqi_dcm_driver/src/motion.cpp

Lines 200 to 216 in 087e9b0

	void Motion::writeJoints(const std::vector <double> &joint_commands)
	{
	//prepare the list of joints
	qi::AnyValue names_qi = fromStringVectorToAnyValue(joints_names_);
	//prepare the list of joint angles
	qi::AnyValue angles_qi = fromDoubleVectorToAnyValue(joint_commands);
	try
	{
	motion_proxy_.async<void>("setAngles", names_qi, angles_qi, 0.2f);
	}
	catch(const std::exception& e)
	{
	ROS_ERROR("Motion: Failed to set joints nagles! \n\tTrace: %s", e.what());
	}
	}

In all the other functions, the input is being sent as is. So when I modify this to send the input directly as a std::vector it works for me. My function now looks like this:

void Motion::writeJoints(const std::vector <double> &joint_commands)
{
  try
  {
    motion_proxy_.async<void>("setAngles", joints_names_, joint_commands, 0.2f);
  }
  catch(const std::exception& e)
  {
    ROS_ERROR("Motion: Failed to set joints nagles! \n\tTrace: %s", e.what());
  }
}

With this, the commands from moveit are being executed on pepper.