Problems Implementing KevinO's ROS Hexapod Stack

Rangwala · October 22, 2019, 7:00am

Hello Everyone

Hello Everyone,

I am working on Trossen Robotics PhantomX Hexapod Mark-III , my aim is to enable navigation and obstacle avoidance on the hexapod. In order to do so, I am using KevinO’s ROS Hexapod Stack. I have already build the stack on my Ubuntu 16.04 ROS Kinetic Linux machine, and tried to pair my Arbotix Commander using the instructions given on the readme page of the stack. But when I run sixpair , the error:

No controller found on USB busses.

pops up. So is it even possible to pair the Arbotix Commander, or the Virtual Commander?
Currently Arbotix-M Robocontroller is being used to control all the 18 servos (AX-18A) of the Hexapod, with the Commander Crawler Code uploaded on it. To use the ROS Hexapod Stack, I had added the following lines at the beginning on the hexapod_ros-master/hexapod_description/params/phantomX.yaml file:

port: /dev/ttyUSB0
read_rate: 15
write_rate: 25

Then I connected the Hexapod to Linux machine using the FTDI cable, controlling it via the Virtual Commander, and ran the following command in the hexapod node:

roslaunch hexapod_bringup hexapod_full.launch config:=phantomX joy_mapping:=joystick_ds3

Then RVIZ launched automatically. After which I made the real hexapod walk using the virtual commander to see if the simulated one on rviz moves as well, but the simulated one did not. In the terminal this was the output:

>     roslaunch hexapod_bringup hexapod_full.launch config:=phantomX joy_mapping:=joystick_ds3
>     ... logging to /home/taher-rangwala/.ros/log/0bed78a2-f3d0-11e9-a248-5800e3d73299/roslaunch-taherrangwala-Lenovo-ideapad-510-15IKB-9868.log
>     Checking log directory for disk usage. This may take awhile.
>     Press Ctrl-C to interrupt
>     Done checking log file disk usage. Usage is <1GB.
> 
> 
>     redefining global property: pi
>     when processing file: /home/taher-rangwala/hexapod_node/src/hexapod_ros-master/hexapod_description/urdf/phantomX_model.xacro
>     started roslaunch server http://134.61.165.118:34275/
> 
> 
>     SUMMARY
>     ========
> 
> 
>     CLEAR PARAMETERS
>     * /ekf_localization/
> 
> 
>     PARAMETERS
>     * /ALARM_LED: 17
>     * /ALARM_SHUTDOWN: 18
>     * /AX_CCW_COMPLIANCE_MARGIN: 27
>     * /AX_CCW_COMPLIANCE_SLOPE: 29
>     * /AX_CW_COMPLIANCE_MARGIN: 26
>     * /AX_CW_COMPLIANCE_SLOPE: 28
> 
>     .....
> 
> 
>     NODES
>     /camera/
>     camera_nodelet_manager (nodelet/nodelet)
>     depth_metric (nodelet/nodelet)
>     depth_metric_rect (nodelet/nodelet)
>     depth_points (nodelet/nodelet)
>     depth_rectify_depth (nodelet/nodelet)
>     depth_registered_sw_metric_rect (nodelet/nodelet)
>     driver (nodelet/nodelet)
>     points_xyzrgb_sw_registered (nodelet/nodelet)
>     register_depth_rgb (nodelet/nodelet)
>     rgb_rectify_color (nodelet/nodelet)
>     /rtabmap/
>     data_odom_sync (nodelet/nodelet)
>     rtabmap (rtabmap_ros/rtabmap)
>     standalone_nodelet (nodelet/nodelet)
>     /
>     Hexapod_Controller (hexapod_controller/hexapod_controller)
>     Hexapod_Sounds (hexapod_sound/hexapod_sound)
>     Hexapod_Teleop_Joystick (hexapod_teleop_joystick/hexapod_teleop_joystick)
>     ImuFilterNodelet (nodelet/nodelet)
>     PhidgetsImuNodelet (nodelet/nodelet)
>     Sound_Play (sound_play/soundplay_node.py)
>     depthimage_to_laserscan (nodelet/nodelet)
>     ekf_localization (robot_localization/ekf_localization_node)
>     imu_manager (nodelet/nodelet)
>     ps4 (ps4_ros/ps4_ros)
>     ps4_joy (joy/joy_node)
>     robot_state_publisher (robot_state_publisher/state_publisher)
>     rviz (rviz/rviz)
> 
> 
>     auto-starting new master
>     process[master]: started with pid [9881]
>     ROS_MASTER_URI=http://134.61.165.118:11311
> 
> 
>     setting /run_id to 0bed78a2-f3d0-11e9-a248-5800e3d73299
>     process[rosout-1]: started with pid [9894]
>     started core service [/rosout]
>     process[Sound_Play-2]: started with pid [9898]
>     process[Hexapod_Sounds-3]: started with pid [9899]
>     process[ps4_joy-4]: started with pid [9900]
>     ERROR: cannot launch node of type [ps4_ros/ps4_ros]: ps4_ros
>     ROS path [0]=/opt/ros/kinetic/share/ros
>     ROS path [1]=/home/taher-rangwala/hexapod_node/src
>     ROS path [2]=/opt/ros/kinetic/share
>     process[Hexapod_Teleop_Joystick-6]: started with pid [9901]
>     process[Hexapod_Controller-7]: started with pid [9902]
>     process[robot_state_publisher-8]: started with pid [9905]
>     process[rviz-9]: started with pid [9906]
>     process[imu_manager-10]: started with pid [9907]
>     process[PhidgetsImuNodelet-11]: started with pid [9908]
>     process[ImuFilterNodelet-12]: started with pid [9909]
>     process[ekf_localization-13]: started with pid [9910]
>     process[camera/camera_nodelet_manager-14]: started with pid [9911]
>     process[camera/driver-15]: started with pid [9912]
>     process[camera/rgb_rectify_color-16]: started with pid [9913]
>     process[camera/depth_rectify_depth-17]: started with pid [9914]
>     process[camera/depth_metric_rect-18]: started with pid [9915]
>     process[camera/depth_metric-19]: started with pid [9916]
>     process[camera/depth_points-20]: started with pid [9917]
>     process[camera/register_depth_rgb-21]: started with pid [9918]
>     process[camera/points_xyzrgb_sw_registered-22]: started with pid [9919]
>     process[camera/depth_registered_sw_metric_rect-23]: started with pid [9920]
>     process[depthimage_to_laserscan-24]: started with pid [9921]
>     process[rtabmap/standalone_nodelet-25]: started with pid [9922]
>     process[rtabmap/data_odom_sync-26]: started with pid [9923]
>     process[rtabmap/rtabmap-27]: started with pid [9950]
>     [ERROR] [1571641053.399303523]: Couldn't open joystick /dev/input/js1. Will retry every second.
>     [ INFO] [1571641053.514859065]: Succeeded to open the port!
>     [ INFO] [1571641053.515074410]: Succeeded to change the baudrate!
>     [ INFO] [1571641054.923710647]: Initializing nodelet with 4 worker threads.
>     [ERROR] [1571641054.976222162]: Failed to load nodelet [/PhidgetsImuNodelet] of type [phidgets_imu/PhidgetsImuNodelet] even after refreshing the cache: According to the loaded plugin descriptions the class phidgets_imu/PhidgetsImuNodelet with base class type nodelet::Nodelet does not exist. Declared types are SlamGMappingNodelet depth_image_proc/convert_metric depth_image_proc/crop_foremost depth_image_proc/disparity depth_image_proc/point_cloud_xyz depth_image_proc/point_cloud_xyz_radial depth_image_proc/point_cloud_xyzi depth_image_proc/point_cloud_xyzi_radial depth_image_proc/point_cloud_xyzrgb depth_image_proc/register depthimage_to_laserscan/DepthImageToLaserScanNodelet hexapod_sound/hexapod_sound hexapod_teleop_joystick/hexapod_teleop_joystick image_proc/crop_decimate image_proc/crop_nonZero image_proc/crop_non_zero image_proc/debayer image_proc/rectify image_proc/resize image_publisher/image_publisher image_rotate/image_rotate image_view/disparity image_view/image imu_filter_madgwick/ImuFilterNodelet nodelet_tutorial_math/Plus openni2_camera/OpenNI2DriverNodelet pcl/BAGReader pcl/BoundaryEstimation pcl/ConvexHull2D pcl/CropBox pcl/EuclideanClusterExtraction pcl/ExtractIndices pcl/ExtractPolygonalPrismData pcl/FPFHEstimation pcl/FPFHEstimationOMP pcl/MomentInvariantsEstimation pcl/MovingLeastSquares pcl/NodeletDEMUX pcl/NodeletMUX pcl/NormalEstimation pcl/NormalEstimationOMP pcl/NormalEstimationTBB pcl/PCDReader pcl/PCDWriter pcl/PFHEstimation pcl/PassThrough pcl/PointCloudConcatenateDataSynchronizer pcl/PointCloudConcatenateFieldsSynchronizer pcl/PrincipalCurvaturesEstimation pcl/ProjectInliers pcl/RadiusOutlierRemoval pcl/SACSegmentation pcl/SACSegmentationFromNormals pcl/SHOTEstimation pcl/SHOTEstimationOMP pcl/SegmentDifferences pcl/StatisticalOutlierRemoval pcl/VFHEstimation pcl/VoxelGrid rtabmap_ros/data_odom_sync rtabmap_ros/data_throttle rtabmap_ros/disparity_to_depth rtabmap_ros/icp_odometry rtabmap_ros/imu_to_tf rtabmap_ros/obstacles_detection rtabmap_ros/obstacles_detection_old rtabmap_ros/point_cloud_aggregator rtabmap_ros/point_cloud_assembler rtabmap_ros/point_cloud_xyz rtabmap_ros/point_cloud_xyzrgb rtabmap_ros/pointcloud_to_depthimage rtabmap_ros/rgbd_odometry rtabmap_ros/rgbd_relay rtabmap_ros/rgbd_sync rtabmap_ros/rgbdicp_odometry rtabmap_ros/rtabmap rtabmap_ros/stereo_odometry rtabmap_ros/stereo_sync rtabmap_ros/stereo_throttle rtabmap_ros/undistort_depth stereo_image_proc/disparity stereo_image_proc/point_cloud2
>     [ERROR] [1571641054.976276546]: The error before refreshing the cache was: According to the loaded plugin descriptions the class phidgets_imu/PhidgetsImuNodelet with base class type nodelet::Nodelet does not exist. Declared types are SlamGMappingNodelet depth_image_proc/convert_metric depth_image_proc/crop_foremost depth_image_proc/disparity depth_image_proc/point_cloud_xyz depth_image_proc/point_cloud_xyz_radial depth_image_proc/point_cloud_xyzi depth_image_proc/point_cloud_xyzi_radial depth_image_proc/point_cloud_xyzrgb depth_image_proc/register depthimage_to_laserscan/DepthImageToLaserScanNodelet hexapod_sound/hexapod_sound hexapod_teleop_joystick/hexapod_teleop_joystick image_proc/crop_decimate image_proc/crop_nonZero image_proc/crop_non_zero image_proc/debayer image_proc/rectify image_proc/resize image_publisher/image_publisher image_rotate/image_rotate image_view/disparity image_view/image imu_filter_madgwick/ImuFilterNodelet nodelet_tutorial_math/Plus openni2_camera/OpenNI2DriverNodelet pcl/BAGReader pcl/BoundaryEstimation pcl/ConvexHull2D pcl/CropBox pcl/EuclideanClusterExtraction pcl/ExtractIndices pcl/ExtractPolygonalPrismData pcl/FPFHEstimation pcl/FPFHEstimationOMP pcl/MomentInvariantsEstimation pcl/MovingLeastSquares pcl/NodeletDEMUX pcl/NodeletMUX pcl/NormalEstimation pcl/NormalEstimationOMP pcl/NormalEstimationTBB pcl/PCDReader pcl/PCDWriter pcl/PFHEstimation pcl/PassThrough pcl/PointCloudConcatenateDataSynchronizer pcl/PointCloudConcatenateFieldsSynchronizer pcl/PrincipalCurvaturesEstimation pcl/ProjectInliers pcl/RadiusOutlierRemoval pcl/SACSegmentation pcl/SACSegmentationFromNormals pcl/SHOTEstimation pcl/SHOTEstimationOMP pcl/SegmentDifferences pcl/StatisticalOutlierRemoval pcl/VFHEstimation pcl/VoxelGrid rtabmap_ros/data_odom_sync rtabmap_ros/data_throttle rtabmap_ros/disparity_to_depth rtabmap_ros/icp_odometry rtabmap_ros/imu_to_tf rtabmap_ros/obstacles_detection rtabmap_ros/obstacles_detection_old rtabmap_ros/point_cloud_aggregator rtabmap_ros/point_cloud_assembler rtabmap_ros/point_cloud_xyz rtabmap_ros/point_cloud_xyzrgb rtabmap_ros/pointcloud_to_depthimage rtabmap_ros/rgbd_odometry rtabmap_ros/rgbd_relay rtabmap_ros/rgbd_sync rtabmap_ros/rgbdicp_odometry rtabmap_ros/rtabmap rtabmap_ros/stereo_odometry rtabmap_ros/stereo_sync rtabmap_ros/stereo_throttle rtabmap_ros/undistort_depth stereo_image_proc/disparity stereo_image_proc/point_cloud2
>     [FATAL] [1571641054.976407288]: Failed to load nodelet '/PhidgetsImuNodelet` of type `phidgets_imu/PhidgetsImuNodelet` to manager `imu_manager'
>     [PhidgetsImuNodelet-11] process has died [pid 9908, exit code 255, cmd /opt/ros/kinetic/lib/nodelet/nodelet load phidgets_imu/PhidgetsImuNodelet imu_manager __name:=PhidgetsImuNodelet __log:=/home/taher-rangwala/.ros/log/0bed78a2-f3d0-11e9-a248-5800e3d73299/PhidgetsImuNodelet-11.log].
>     log file: /home/taher-rangwala/.ros/log/0bed78a2-f3d0-11e9-a248-5800e3d73299/PhidgetsImuNodelet-11*.log
>     [ INFO] [1571641055.943828798]: No matching device found.... waiting for devices. Reason: std::__cxx11::string openni2_wrapper::OpenNI2Driver::resolveDeviceURI(c onst string&) @ /tmp/binarydeb/ros-kinetic-openni2-camera-0.4.2/src/openni2_driver.cpp @ 737 : Invalid device number 1, there are 0 devices connected.

Here is a screenshot of the RVIZ simulation:

According to the terminal output (in Bold) it’s succeeded at opening the port, but the rviz simulation is not connected to the real hexapod. Does anyone know how to connect the two?

I have a Sparkfun 9DoF Razor IMU for odometry and a Sick-Tim 2D Lidar Laserscanner that will be used to generate a map, and also a Raspberry Pi 3 with ROS Kinetic on it. My plan is to connect the IMU and Laserscanner to the RPi3 with the relevant ROS pacakges to get data streams, and then ssh into the pi from my Linux machine that runs the hexapod stack. This is because in my opinion Rpi3 is not suitable to run Rviz and such simulations.

By the way, I have already implemented the arbotix_ros stack, hence I am able to control all the servos at the same time using the gui, and
by writing a python script to control joint positions. So I was wondering do I have to upload any sketch on the Arbotix-M board for the ROS Hexapod Stack to function? since I had to do so for the arbotix_ros stack

I will appreciate any help on implementing KevinO’s ROS Hexapod stack, as I am sure many on this forum have already done so. I am happy to try out anything needed, as I have read that people have used the USB2Dynamixel or USB2 AX to control the servos, but I am not sure if it’s needed in my case. Let me know if any other information is required.

Thank you!
Taher Rangwala

RoboCS · October 25, 2019, 1:25pm

Hello @Rangwala !

Pretty great project, I’ll look forward to see it under the robot’s section once its done. Talking about your project, Could you verify some things to me please:

Verify that all topics that comes from the hexapod are really being publish:

 cmd_vel (geometry_msgs/Twist) Velocity command. 
 body_scalar (geometry_msgs::AccelStamped) Scalar to modifiy the orientation of the body.
 head_scalar (geometry_msgs::AccelStamped) Scalar to modifiy the pan and tilt of the optional turret.
 state (std_msgs::Bool) Bool array to record state of the hexapod. Standing up, sitting etc.
 imu/data (sensor_msgs::Imu) Used in optional auto body leveling on non level ground.

After that, verify that your package topic’s are being published all too:

sounds (hexapod_msgs::Sounds) Custom message to send sound cues to the optional sound package.
joint_states (sensor_msgs::JointState) Joint states for rviz and such.
odometry/calculated (nav_msgs::Odometry) Calculated odometry from the gait system in the package.
twist (geometry_msgs::TwistWithCovarianceStamped) Twist message syncronized with the gait system.

With this we can check why your robot is not moving in rviz. Also, you should check those errors over the model in rviz, usually, they carry errors of their own.

Rangwala · October 25, 2019, 2:20pm

Hello @RoboCS,

Thanks for replying, everything seems to be published correctly except:
state publishes:
data: False
imu/data is not being published yet, because I have not connected it yet.
Also sound is not being published.

I have already looked into the errors in RVIZ’s Display panel. Here’s a screenshot of the extended version. As you can see TF shows the same errors as RobotModel:

If you see the rqt_graph:

you can see that a controller (ex:ps4 or xbox) is supposed to be paired, but I only have the Arbotix Commander and Virtual Commander. I tried to pair both using this tutorial in ROS
but a jsX port did not show up. So instead I am directly publishing cmd_vel commands using my keyboard. This was done by implementing another ROS stack: teleop-twist-keyboard

Do you have any other ideas or suggestions?

Thank you,
Taher Rangwala