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.
|
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.
React
Typescript
Django
Laravel
Facebook
Microsoft
Google
Alibaba
D3
Tencent