zebradevs/robot_controllers

Robot control infrastructure

Robot Controllers

This is a robot control infrastructure, developed initially for Fetch and Freight, but designed to be robot-agnostic. In comparison to ros_control, robot_controllers offers the ability to "stack" controllers and avoids template-based hardware interfaces. robot_controllers has not be designed with real-time constraints in mind, and is intended primarily for robots where the real-time joint-level controllers are run in hardware, as is the case for Fetch and Freight.

• Noetic Devel Job Status:
• Noetic AMD64 Debian Job Status:

robot_controllers's Issues

Member variable updated after return statement

I was looking through robot controllers and was trying to understand the code contained in the file pid.cpp. I found that the member variable error_last_ was updated after a return statement:

https://github.com/fetchrobotics/robot_controllers/blob/indigo-devel/robot_controllers/src/pid.cpp#L161

This seemed like a problem to me unless there is a C++ trick that I don't know about

Add List Controllers service API to controller manager

ros-control allows you to make a service call to get the active controllers.

The name is: "/robot_name/controller_manager/list_controllers"
and the type is controller_manager_msgs::ListControllers

It would be helpful for the ros-controllers controller manager to provide a similar functionality.

How to use robot_controllers with a non-Fetch robot

We have been using the diff_drive_controller/DiffDriveController plugin in Gazebo 2.3 under ROS Indigo with a model of our custom robot (Jackrabbot) at Stanford University. However, we have discovered that the robot rotates very slowly in Gazebo even with no Twist control input, in an empty world, and no matter how we set the pose and twist covariances.

We therefore wanted to try the robot_controllers/DiffDriveBaseController provided here since I noticed that the Fetch Gazebo model (also under ROS Indigo and Gazebo 2.3) does not have this rotation problem.

I include my YAML file below but the DiffDriveBaseController plugin is not being loaded when spawning our Jackrabbot model in Gazebo. I noticed that with the Fetch gazebo launch file, the following lines at the end of the file fetch.gazebo.xacro:

<gazebo>
    <plugin name="fetch_gazebo_plugin" filename="libfetch_gazebo_plugin.so"/>
</gazebo>

are necessary for the DiffDriveBaseController plugin to be loaded. My question is therefore, how can I get the DiffDriveBaseController plugin to load without using the fetch_gazebo_plugin which I am guessing is specific to the Fetch robot?

Thank you! Here is my controllers yaml file:

base_controller:
  type: "robot_controllers/DiffDriveBaseController"
  autostart: true
  base_frame: sibot/base_link
  l_wheel_joint: "left_wheel"
  r_wheel_joint: "right_wheel"
  track_width: 0.542391
  radians_per_meter: 4.15682711
  publish_tf: true
  max_velocity_x: 3.57632
  max_velocity_r: 4.5
  max_acceleration_x: 0.75
  max_acceleration_r: 3.0
  publish_frequency: 100
  
gazebo:
  default_controllers:
    - "base_controller"
  l_wheel_joint:
    position:
      p: 0.0
      d: 0.0
      i: 0.0
      i_clamp: 0.0
    velocity:
      p: 8.85
      d: 0.0
      i: 0.5
      i_clamp: 6.0
  r_wheel_joint:
    position:
      p: 0.0
      d: 0.0
      i: 0.0
      i_clamp: 0.0
    velocity:
      p: 8.85
      d: 0.0
      i: 0.5
      i_clamp: 6.0

private ControllerList in the controller manager

Hi I was wondering what the reason was to have the ControllerList private in the ControllerManager class. I'm trying to work with this class, but keep wanting more info about controllers. Since I can't access the list, and since i can only get the pointers to the handles for the controllers, it's proving difficult to get the info i need. I can get the joint handles by name, wondering why i cant actualyl get the controllers (as controller classes) by name. Thanks.

Change default branch to ros1?

@erelson should we change the default branch to ros1? (Which AFAIK will actually work on both melodic + noetic, which are the only supported ROS1 distros out there).

I was going to open a PR for some additional documentation so we can close a few issues that are mainly asking questions, planning to open against ros1 and would like that README stuff to be on the front page.

How to enable Cartesian Pose Controller in fetch driver

Hi there,
in robot_controllers pkg, there is a class called CartesianPoseController.
It seems that this function has not been activated in fetch_drivers or fetch_bringup, may I know how I can use this class? since I think it may be useful to execute a short straight line path directly without external planner (such as by Moveit!)

Comments are welcome!

Cannot start controller after stopping controller manually

Hi. When I used https://github.com/fetchrobotics/robot_controllers/blob/010855f0e6a8e058a1cbd2ad69c324cd97f0d815/robot_controllers_interface/scripts/stop_controller.py to stop a controller and tried to start the controller by using https://github.com/fetchrobotics/robot_controllers/blob/010855f0e6a8e058a1cbd2ad69c324cd97f0d815/robot_controllers_interface/scripts/start_controller.py, I got the following error and did not start the controller.

fetch@fetch1075:~/ros/melodic/src/fetchrobotics/robot_controllers/robot_controllers_interface$ rosrun robot_controllers_interface stop_controller.py arm_controller/follow_joint_trajectory
[INFO] [WallTime: 1636960585.632305] [node:/stop_robot_controllers] [func:<module>]: Connecting to /query_controller_states...
[INFO] [WallTime: 1636960585.794209] [node:/stop_robot_controllers] [func:<module>]: Done.
[INFO] [WallTime: 1636960585.795474] [node:/stop_robot_controllers] [func:<module>]: Requesting that arm_controller/follow_joint_trajectory be stopped...
[INFO] [WallTime: 1636960585.797601] [node:/stop_robot_controllers] [func:<module>]: Done.
fetch@fetch1075:~/ros/melodic/src/fetchrobotics/robot_controllers/robot_controllers_interface$ rosrun robot_controllers_interface start_controller.py arm_controller/follow_joint_trajectory 
[INFO] [WallTime: 1636960615.499979] [node:/start_robot_controllers] [func:<module>]: Connecting to /query_controller_states...
[INFO] [WallTime: 1636960615.754447] [node:/start_robot_controllers] [func:<module>]: Done.
[INFO] [WallTime: 1636960615.756414] [node:/start_robot_controllers] [func:<module>]: Requesting that arm_controller/follow_joint_trajectory be started...
[ERROR] [WallTime: 1636960615.759133] [node:/start_robot_controllers] [func:<module>]: Unable to start arm_controller/follow_joint_trajectory

/var/log/ros/robot.log output

[ERROR] [1636960615.757763468] [/robot_driver:ros.robot_controllers.FollowJointTrajectoryController]: Unable to start, action server is not active.

https://github.com/fetchrobotics/robot_controllers/blob/010855f0e6a8e058a1cbd2ad69c324cd97f0d815/robot_controllers/src/follow_joint_trajectory.cpp#L145-L150
I wonder if checking isActive() is proper when starting contollers.
I think isActive() shows status whether an active goal already exists or not.
https://github.com/ros/actionlib/blob/10025114a28d94581b6fac68bd391eb2a1bc3d32/include/actionlib/server/simple_action_server_imp.h#L227-L236

Cc:@708yamaguchi @knorth55

ROS2 Port

I've started working on this (https://github.com/mikeferguson/robot_controllers/tree/ros2) but figured I'd post a note here until we get a ros2 branch going on this repo.

Roadmap:

• Port robot_controller_msgs
• Port robot_controllers_interface
  • Port ControllerLoader
  • Port ControllerManager
  • Port Handle and derived classes
  • Move Controller, Handle(s), Manager, Loader classes into robot_controllers_interface namespace (this was arguably a bug in the original ROS1, which required all downstream controllers to depend on robot_controllers rather than robot_controllers_interface).
  • Change ControllerManager to use service rather than action. The interface is a lot cleaner, and in ROS2 services can be asynchronous, so the deadlock fears in ROS1 are gone.
  • Port python scripts
• Port robot_controllers
  • cartesian_pose
  • cartesian_twist
  • cartesian_wrench
  • diff_drive_controller - planning to still tweak a few things still
  • follow_joint_trajectory
  • gravity_compensation
  • parallel_gripper - ported, not yet fully tested
  • pid
  • point_head
  • scaled_mimic
  • trajectory tests
• Add documentation to README that shows how to use updated interfaces

Bugs/Improvements to Look At:

• Need to test cancellation of actions - probably need to change aborted() to cancelled() for them as well.
• Add a dynamic parameter interface to the PID class. Moved to #50

Consider equal forces for gripper jaws

The parallel jaw gripper plugin is currently position-based. It tends to slide off to one side or the other when grasping. We should try running a single PID loop in the controller and setting the joint efforts directly (and to equal values).

Bug in "follow_joint_trajectory.cpp" ?

Hi, Can you double check if the following code (from line 456 -475) in the file "follow_joint_trajectory.cpp " has a bug?

 // Convert the path tolerances into a more usable form
    for (size_t j = 0; j < joints_.size(); ++j)
    {
      int index = -1;
      for (size_t i = 0; i < goal->path_tolerance.size(); ++i)
      {
        if (joints_[j]->getName() == goal->path_tolerance[i].name)
        {
          index = i;
          break;
        } //??????
        if (index == -1)
        {
          result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
          server_->setAborted(result, "Unable to convert path tolerances");
          ROS_ERROR("Unable to convert path tolerances");
          return;
        }
        path_tolerance_.q[j] = goal->path_tolerance[i].position;
        path_tolerance_.qd[j] = goal->path_tolerance[i].velocity;
        path_tolerance_.qdd[j] = goal->path_tolerance[i].acceleration;
      }//end second for
    } //end first for

To me, the following changes make more sense

    for (size_t j = 0; j < joints_.size(); ++j)
    {
      int index = -1;
      for (size_t i = 0; i < goal->path_tolerance.size(); ++i)
      {
        if (joints_[j]->getName() == goal->path_tolerance[i].name)
        {
          index = i;
          break;
        }
      } // end second for
        if (index == -1)
        {
          result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
          server_->setAborted(result, "Unable to convert path tolerances");
          ROS_ERROR("Unable to convert path tolerances");
          return;
        }
        path_tolerance_.q[j] = goal->path_tolerance[index].position;
        path_tolerance_.qd[j] = goal->path_tolerance[index].velocity;
        path_tolerance_.qdd[j] = goal->path_tolerance[index].acceleration;

    } //end first for

And similar question for the part : // Convert the goal tolerances into a more usable form

[Question] Controller stacking mechanics

I've been looking into the architecture of the robot_controllers project, and have had some trouble figuring out how controlling stacking works. I see that:

1. robot_controllers::Controller allows controllers to have access to other controllers by querying the robot_controllers::ControllerManager for a handle that can be dynamic_cast'ed to a controller.
2. robot_controllers::ControllerManager is responsible for updating all controllers.

but I'm not sure I understand the usage pattern, and have failed to find examples.

I can see that a controller can use another controller's interface, but there seems to be no mechanism to control the order in which controllers get updated, other than the order in which they are loaded. My understanding is that all controllers live in a "controller soup", with no explicit topological dependencies between them to enforce that every controller has the most recent input. In the "controller soup" scenario, a controller stack of size n has a worst-case delay of n control cycles until hardware state readings propagate to the last controller in the stack.

Am I interpreting this correctly?.

How to control the value of "gripper_finger_joint" using "robot_controllers::FollowJointTrajectoryController"

Hi,
I'm trying to add "gripper_finger_joint" to "robot_controllers::FollowJointTrajectoryController" and control it at the same time as the other joints, but it fails.

The JointHandlePtr is obtained with https://github.com/fetchrobotics/robot_controllers/blob/ros1/robot_controllers/src/follow_joint_trajectory.cpp#L100-L106
and each joint is controlled using this value. This control includes getPosition, setPosition, and so on.

So I tried to get the value of "gripper_finger_joint" by adding the following two lines to https://github.com/fetchrobotics/fetch_robots/blob/melodic-devel/fetch_bringup/config/default_controllers.yaml#L22-L33.

• l_gripper_finger_joint
• r_gripper_finger_joint

As a result, the value of j in "JointHandlePtr j = manager_->getJointHandle(joint_names_[i])" became zero, and JointHandlePtr could not be obtained.
If it is "gripper_finger_joint", will it be implemented differently from other joints?
Does this implementation usually work?

Thanks in advance.

robot_driver failed with current melodic-devel

robot_driver 0.6.0 fails with current origin/melodic-devel.
I revert 6452011 and build the workspace, and the error disappeared.
I think something is wrong with 6452011.

fetch@fetch15:~$ roslaunch fetch_bringup fetch.launch 
... logging to /home/fetch/.ros/log/0e4ebaa2-3600-11eb-9006-a8a15943aefb/roslaunch-fetch15-15802.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://fetch15:34527/

SUMMARY
========

PARAMETERS
 * /arm_controller/cartesian_twist/root_name: torso_lift_link
 * /arm_controller/cartesian_twist/tip_name: wrist_roll_link
 * /arm_controller/cartesian_twist/type: robot_controllers...
 * /arm_controller/follow_joint_trajectory/joints: ['shoulder_pan_jo...
 * /arm_controller/follow_joint_trajectory/type: robot_controllers...
 * /arm_controller/gravity_compensation/autostart: True
 * /arm_controller/gravity_compensation/root: torso_lift_link
 * /arm_controller/gravity_compensation/tip: gripper_link
 * /arm_controller/gravity_compensation/type: robot_controllers...
 * /arm_with_torso_controller/follow_joint_trajectory/joints: ['torso_lift_join...
 * /arm_with_torso_controller/follow_joint_trajectory/type: robot_controllers...
 * /base_controller/autostart: True
 * /base_controller/laser_safety_dist: 1.0
 * /base_controller/max_acceleration_x: 0.75
 * /base_controller/max_velocity_x: 1.0
 * /base_controller/moving_threshold: -0.01
 * /base_controller/publish_tf: False
 * /base_controller/rotating_threshold: -0.01
 * /base_controller/track_width: 0.37476
 * /base_controller/type: robot_controllers...
 * /calibration_date: uncalibrated
 * /diagnostic_aggregator/analyzers/breakers/analyzers/arm_breaker/path: Arm Breaker
 * /diagnostic_aggregator/analyzers/breakers/analyzers/arm_breaker/startswith: arm_breaker
 * /diagnostic_aggregator/analyzers/breakers/analyzers/arm_breaker/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/breakers/analyzers/base_breaker/path: Base Breaker
 * /diagnostic_aggregator/analyzers/breakers/analyzers/base_breaker/startswith: base_breaker
 * /diagnostic_aggregator/analyzers/breakers/analyzers/base_breaker/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/breakers/analyzers/battery_breaker/path: Battery Breaker
 * /diagnostic_aggregator/analyzers/breakers/analyzers/battery_breaker/startswith: battery_breaker
 * /diagnostic_aggregator/analyzers/breakers/analyzers/battery_breaker/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/breakers/analyzers/computer_breaker/path: Computer Breaker
 * /diagnostic_aggregator/analyzers/breakers/analyzers/computer_breaker/startswith: computer_breaker
 * /diagnostic_aggregator/analyzers/breakers/analyzers/computer_breaker/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/breakers/analyzers/gripper_breaker/path: Gripper Breaker
 * /diagnostic_aggregator/analyzers/breakers/analyzers/gripper_breaker/startswith: gripper_breaker
 * /diagnostic_aggregator/analyzers/breakers/analyzers/gripper_breaker/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/breakers/analyzers/supply_breaker/path: Supply Breaker
 * /diagnostic_aggregator/analyzers/breakers/analyzers/supply_breaker/startswith: supply_breaker
 * /diagnostic_aggregator/analyzers/breakers/analyzers/supply_breaker/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/breakers/path: Breakers
 * /diagnostic_aggregator/analyzers/breakers/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/motors/analyzers/elbow_flex/path: Elbow Flex Joint
 * /diagnostic_aggregator/analyzers/motors/analyzers/elbow_flex/startswith: elbow_flex
 * /diagnostic_aggregator/analyzers/motors/analyzers/elbow_flex/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/motors/analyzers/forearm_roll/path: Forearm Roll Joint
 * /diagnostic_aggregator/analyzers/motors/analyzers/forearm_roll/startswith: forearm_roll
 * /diagnostic_aggregator/analyzers/motors/analyzers/forearm_roll/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/motors/analyzers/head_pan/path: Head Pan Joint
 * /diagnostic_aggregator/analyzers/motors/analyzers/head_pan/startswith: head_pan
 * /diagnostic_aggregator/analyzers/motors/analyzers/head_pan/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/motors/analyzers/head_tilt/path: Head Tilt Joint
 * /diagnostic_aggregator/analyzers/motors/analyzers/head_tilt/startswith: head_tilt
 * /diagnostic_aggregator/analyzers/motors/analyzers/head_tilt/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/motors/analyzers/l_wheel/path: Left Wheel
 * /diagnostic_aggregator/analyzers/motors/analyzers/l_wheel/startswith: l_wheel
 * /diagnostic_aggregator/analyzers/motors/analyzers/l_wheel/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/motors/analyzers/r_wheel/path: Right Wheel
 * /diagnostic_aggregator/analyzers/motors/analyzers/r_wheel/startswith: r_wheel
 * /diagnostic_aggregator/analyzers/motors/analyzers/r_wheel/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/motors/analyzers/shoulder_lift/path: Shoulder Lift Joint
 * /diagnostic_aggregator/analyzers/motors/analyzers/shoulder_lift/startswith: shoulder_lift
 * /diagnostic_aggregator/analyzers/motors/analyzers/shoulder_lift/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/motors/analyzers/shoulder_pan/path: Shoulder Pan Joint
 * /diagnostic_aggregator/analyzers/motors/analyzers/shoulder_pan/startswith: shoulder_pan
 * /diagnostic_aggregator/analyzers/motors/analyzers/shoulder_pan/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/motors/analyzers/torso_lift/path: Torso Lift Joint
 * /diagnostic_aggregator/analyzers/motors/analyzers/torso_lift/startswith: torso_lift
 * /diagnostic_aggregator/analyzers/motors/analyzers/torso_lift/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/motors/analyzers/upperarm_roll/path: Upperarm Roll Joint
 * /diagnostic_aggregator/analyzers/motors/analyzers/upperarm_roll/startswith: upperarm_roll
 * /diagnostic_aggregator/analyzers/motors/analyzers/upperarm_roll/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/motors/analyzers/wrist_flex/path: Wrist Flex Joint
 * /diagnostic_aggregator/analyzers/motors/analyzers/wrist_flex/startswith: wrist_flex
 * /diagnostic_aggregator/analyzers/motors/analyzers/wrist_flex/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/motors/analyzers/wrist_roll/path: Wrist Roll Joint
 * /diagnostic_aggregator/analyzers/motors/analyzers/wrist_roll/startswith: wrist_roll
 * /diagnostic_aggregator/analyzers/motors/analyzers/wrist_roll/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/motors/path: Motor Control Boards
 * /diagnostic_aggregator/analyzers/motors/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/peripherals/analyzers/joy/find_and_remove_prefix: joy:
 * /diagnostic_aggregator/analyzers/peripherals/analyzers/joy/path: PS3 Controller
 * /diagnostic_aggregator/analyzers/peripherals/analyzers/joy/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/peripherals/path: Peripherals
 * /diagnostic_aggregator/analyzers/peripherals/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/sensors/analyzers/base_breaker/path: Sick TIM551 Laser
 * /diagnostic_aggregator/analyzers/sensors/analyzers/base_breaker/startswith: sick_tim
 * /diagnostic_aggregator/analyzers/sensors/analyzers/base_breaker/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/sensors/path: Sensors
 * /diagnostic_aggregator/analyzers/sensors/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/sound_play/path: SoundPlay
 * /diagnostic_aggregator/analyzers/sound_play/startswith: sound_play
 * /diagnostic_aggregator/analyzers/sound_play/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/system/analyzers/charger/path: Charger
 * /diagnostic_aggregator/analyzers/system/analyzers/charger/startswith: Charger
 * /diagnostic_aggregator/analyzers/system/analyzers/charger/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/system/analyzers/mainboard/path: Mainboard
 * /diagnostic_aggregator/analyzers/system/analyzers/mainboard/startswith: Mainboard
 * /diagnostic_aggregator/analyzers/system/analyzers/mainboard/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/system/path: System
 * /diagnostic_aggregator/analyzers/system/type: diagnostic_aggreg...
 * /diagnostic_aggregator/base_path: 
 * /diagnostic_aggregator/pub_rate: 1.0
 * /graft/alpha: 0.001
 * /graft/beta: 2.0
 * /graft/child_frame_id: base_link
 * /graft/kappa: 0.0
 * /graft/output_frame: odom
 * /graft/parent_frame_id: odom
 * /graft/planar_output: True
 * /graft/process_noise: ['1e6', 0, 0, 0, ...
 * /graft/publish_tf: True
 * /graft/queue_size: 1
 * /graft/topics/imu/absolute_orientation: False
 * /graft/topics/imu/no_delay: True
 * /graft/topics/imu/override_angular_velocity_covariance: [0, 0, 0, 0, 0, 0...
 * /graft/topics/imu/timeout: 1.0
 * /graft/topics/imu/topic: /imu
 * /graft/topics/imu/type: sensor_msgs/Imu
 * /graft/topics/imu/use_accelerations: False
 * /graft/topics/imu/use_velocities: True
 * /graft/topics/odom/absolute_pose: False
 * /graft/topics/odom/no_delay: True
 * /graft/topics/odom/override_twist_covariance: ['1e-3', 0, 0, 0,...
 * /graft/topics/odom/timeout: 1.0
 * /graft/topics/odom/topic: /odom
 * /graft/topics/odom/type: nav_msgs/Odometry
 * /graft/topics/odom/use_velocities: True
 * /graft/update_rate: 50.0
 * /graft/update_topic: odom
 * /gripper_driver/firmware_tar_gz: /opt/ros/melodic/...
 * /head_camera/crop_decimate/decimation_x: 4
 * /head_camera/crop_decimate/decimation_y: 4
 * /head_camera/crop_decimate/queue_size: 1
 * /head_camera/depth_rectify_depth/interpolation: 0
 * /head_camera/depth_registered_rectify_depth/interpolation: 0
 * /head_camera/driver/color_depth_synchronization: False
 * /head_camera/driver/color_mode: 5
 * /head_camera/driver/depth_camera_info_url: 
 * /head_camera/driver/depth_frame_id: head_camera_depth...
 * /head_camera/driver/depth_mode: 5
 * /head_camera/driver/depth_registration: True
 * /head_camera/driver/device_id: #1
 * /head_camera/driver/id_manufacturer: 1d27
 * /head_camera/driver/id_product: 0601
 * /head_camera/driver/ir_mode: 5
 * /head_camera/driver/rgb_camera_info_url: 
 * /head_camera/driver/rgb_frame_id: head_camera_rgb_o...
 * /head_camera/driver/z_offset_mm: 0.0
 * /head_camera/driver/z_scaling: 1.0
 * /head_camera/head_camera_nodelet_manager/num_worker_threads: 4
 * /head_controller/follow_joint_trajectory/joints: ['head_pan_joint'...
 * /head_controller/follow_joint_trajectory/type: robot_controllers...
 * /head_controller/point_head/type: robot_controllers...
 * /joy/deadzone: 0.05
 * /joy_node/autorepeat_rate: 1
 * /joy_node/dev: /dev/fetch_joy
 * /joy_remap/mappings/axes: ['axes[0]', 'axes...
 * /joy_remap/mappings/buttons: ['buttons[8]', 'b...
 * /robot/name: fetch15
 * /robot/type: fetch
 * /robot_description: <robot name="fetc...
 * /robot_driver/default_controllers: ['arm_controller/...
 * /robot_driver/firmware_tar_gz: /opt/ros/melodic/...
 * /robot_driver/has_arm: True
 * /robot_driver/has_base: True
 * /robot_driver/has_head: True
 * /robot_driver/has_torso: True
 * /rosdistro: melodic
 * /rosversion: 1.14.10
 * /sick_tim551_2050001/frame_id: laser_link
 * /sick_tim551_2050001/hostname: 10.42.42.10
 * /sick_tim551_2050001/max_ang: 1.91986
 * /sick_tim551_2050001/min_ang: -1.91986
 * /sick_tim551_2050001/range_max: 25.0
 * /sick_tim551_2050001/time_increment: 6.1728e-05
 * /teleop/use_arm: True
 * /teleop/use_base: True
 * /teleop/use_gripper: True
 * /teleop/use_head: True
 * /teleop/use_torso: True
 * /torso_controller/follow_joint_trajectory/joints: ['torso_lift_joint']
 * /torso_controller/follow_joint_trajectory/type: robot_controllers...

NODES
  /
    auto_dock (fetch_open_auto_dock/auto_dock)
    cmd_vel_mux (topic_tools/mux)
    controller_reset (fetch_bringup/controller_reset.py)
    diagnostic_aggregator (diagnostic_aggregator/aggregator_node)
    dock_on_button (fetch_open_auto_dock/dock_on_button.py)
    graft (graft/graft_ukf_velocity)
    gripper_driver (fetch_drivers/gripper_driver)
    joy_node (joy/joy_node)
    joy_remap (joy/joy_remap.py)
    robot_driver (fetch_drivers/robot_driver)
    robot_state_publisher (robot_state_publisher/state_publisher)
    sick_tim551_2050001 (sick_tim/sick_tim551_2050001)
    sick_tim_filter (fetch_drivers/laser_filter)
    software_runstop (fetch_bringup/software_runstop.py)
    teleop (fetch_teleop/joystick_teleop)
    tuck_arm (fetch_teleop/tuck_arm.py)
    undock_on_button (fetch_open_auto_dock/undock_on_button.py)
  /head_camera/
    crop_decimate (nodelet/nodelet)
    depth_metric (nodelet/nodelet)
    depth_metric_rect (nodelet/nodelet)
    depth_points (nodelet/nodelet)
    depth_rectify_depth (nodelet/nodelet)
    depth_registered_hw_metric_rect (nodelet/nodelet)
    depth_registered_metric (nodelet/nodelet)
    depth_registered_rectify_depth (nodelet/nodelet)
    driver (nodelet/nodelet)
    head_camera_nodelet_manager (nodelet/nodelet)
    points_xyzrgb_hw_registered (nodelet/nodelet)
    rgb_rectify_color (nodelet/nodelet)
  /head_camera/depth_downsample/
    points_downsample (nodelet/nodelet)

auto-starting new master
process[master]: started with pid [15812]
ROS_MASTER_URI=http://fetch15:11311

setting /run_id to 0e4ebaa2-3600-11eb-9006-a8a15943aefb
process[rosout-1]: started with pid [15823]
started core service [/rosout]
process[graft-2]: started with pid [15830]
process[robot_driver-3]: started with pid [15832]
[ INFO] [1607065944.518197467]: Number of topics: 2
[ INFO] [1607065944.518771818]: Topic name: imu
[ INFO] [1607065944.519012180]: Type: sensor_msgs/Imu
process[gripper_driver-4]: started with pid [15837]
[ INFO] [1607065944.522641196]: Abs orientation: 0
Delta orientation: 0
Use Vel: 1
Timeout: 1.000
[ INFO] [1607065944.523315938]: Topic name: odom
[ INFO] [1607065944.523530130]: Type: nav_msgs/Odometry
[ERROR] [1607065944.527074220]: initial_covariance is size 0, expected 9.
Using 0.1*Identity.
This probably won't work well.
[ERROR] [1607065944.529281804]: Can't call mlockall to protect freight_driver!!!
[ERROR] [1607065944.529321051]: Can't call nice to prioritize freight_driver!!!
process[robot_state_publisher-5]: started with pid [15839]
[ERROR] [1607065944.536019135]: Can't call mlockall to protect gripper_driver!!!
[ERROR] [1607065944.536052267]: Can't call nice to prioritize gripper_driver!!!
process[head_camera/head_camera_nodelet_manager-6]: started with pid [15848]
[ WARN] [1607065944.543457546]: The 'state_publisher' executable is deprecated. Please use 'robot_state_publisher' instead
[ WARN] [1607065944.547076219]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
[ WARN] [1607065944.547563535]: Negative dt - odom
process[head_camera/driver-7]: started with pid [15862]
process[head_camera/rgb_rectify_color-8]: started with pid [15865]
process[head_camera/depth_rectify_depth-9]: started with pid [15873]
[ WARN] [1607065944.567651301]: imu (IMU) timeout
[ WARN] [1607065944.567697405]: odom (Odometry) timeout
process[head_camera/depth_metric_rect-10]: started with pid [15881]
process[head_camera/depth_metric-11]: started with pid [15892]
[ INFO] [1607065944.587560953]: Initializing nodelet with 4 worker threads.
process[head_camera/depth_points-12]: started with pid [15903]
process[head_camera/depth_registered_rectify_depth-13]: started with pid [15913]
process[head_camera/points_xyzrgb_hw_registered-14]: started with pid [15919]
process[head_camera/depth_registered_hw_metric_rect-15]: started with pid [15925]
[ WARN] [1607065944.625764239]: Board Not ready: 0x80
process[head_camera/depth_registered_metric-16]: started with pid [15931]
process[head_camera/crop_decimate-17]: started with pid [15938]
[ INFO] [1607065944.647346360]: Loading nodelet /head_camera/crop_decimate of type image_proc/crop_decimate to manager /head_camera/head_camera_nodelet_manager with the following remappings:
[ INFO] [1607065944.647806872]: /head_camera/camera/camera_info -> /head_camera/depth_registered/camera_info
[ INFO] [1607065944.647817468]: /head_camera/camera/image_raw -> /head_camera/depth_registered/image_raw
[ INFO] [1607065944.647823233]: /head_camera/camera_out -> /head_camera/depth_downsample
process[head_camera/depth_downsample/points_downsample-18]: started with pid [15943]
process[sick_tim551_2050001-19]: started with pid [15949]
process[sick_tim_filter-20]: started with pid [15959]
process[joy_node-21]: started with pid [15961]
[ WARN] [1607065944.686538465]: /sick_tim_filter (type=Chain) initialized as an empty filter sequence
process[joy_remap-22]: started with pid [15971]
[ WARN] [1607065944.691208922]: NoFilterBranch (type=Chain) initialized as an empty filter sequence
process[teleop-23]: started with pid [15973]
process[cmd_vel_mux-24]: started with pid [15975]
[ERROR] [1607065944.710504611]: Couldn't open joystick /dev/fetch_joy. Will retry every second.
process[controller_reset-25]: started with pid [15985]
process[tuck_arm-26]: started with pid [15989]
[ WARN] [1607065944.729638554]: IntensityFilterBranch (type=Chain) initialized as an empty filter sequence
process[auto_dock-27]: started with pid [15991]
[ INFO] [1607065944.735737805]: Board ready: 0x80
process[dock_on_button-28]: started with pid [15993]
process[undock_on_button-29]: started with pid [16001]
process[software_runstop-30]: started with pid [16002]
[ INFO] [1607065944.764471198]: Dock perception initialized
process[diagnostic_aggregator-31]: started with pid [16005]
[ INFO] [1607065944.815463141]: Device "1d27/0609@1/2" found.
Warning: USB events thread - failed to set priority. This might cause loss of data...
[ WARN] [1607065944.954546718]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
[ INFO] [1607065944.956869579]: Started arm_controller/gravity_compensation
[ WARN] [1607065944.960272211]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
[ INFO] [1607065944.980291146]: Started base_controller
[ WARN] [1607065944.995450528]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
terminate called after throwing an instance of 'YAML::InvalidNode'
  what():  yaml-cpp: error at line 0, column 0: invalid node; this may result from using a map iterator as a sequence iterator, or vice-versa
[robot_driver-3] process has died [pid 15832, exit code -6, cmd /opt/ros/melodic/lib/fetch_drivers/robot_driver __name:=robot_driver __log:=/home/fetch/.ros/log/0e4ebaa2-3600-11eb-9006-a8a15943aefb/robot_driver-3.log].
log file: /home/fetch/.ros/log/0e4ebaa2-3600-11eb-9006-a8a15943aefb/robot_driver-3*.log

cc. @708yamaguchi @mqcmd196

laser speed limiting should have timeout on loss of signal from laser

trajectory speed is not constant, after sending cancel goal

when we send TrajectoryGoal with 1 positions, send cancel during execution, wait few more seconds, and then send the Trajectory again, the robot moves very quickly.

point2 = JointTrajectoryPoint()
point2.positions = [0,0,0,-1.57,1.57,0,0]
point2.time_from_start = rospy.Duration(10)
goal2.trajectory.points.append(point2)


rospy.logwarn("send goal2")
action.send_goal(goal2)
# sleeep 2                                                                                                                       
rospy.sleep(rospy.Duration(3))
rospy.logwarn("send cancel")
action.cancel_goal()
#                                                                                                                                
rospy.sleep(rospy.Duration(10))
rospy.logwarn("sleep 10sec")

# send goal 2                                                                                                                    
rospy.logwarn("send goal2 again")
action.send_goal_and_wait(goal2)

script to reproduce this behavior is https://gist.github.com/708yamaguchi/324b7ef246175619cc3fe7fe7e4bac76

and joint position profile during this experiment is

Our first guess is something related to
https://github.com/fetchrobotics/robot_controllers/blob/indigo-devel/robot_controllers/src/follow_joint_trajectory.cpp#L400

Cc: @knorth55, @k-okada

How to enable a custom effort-based controller in fetch driver

Hi,
I have been trying to implement a custom effort/torque-based controller for the Fetch robot arm and have been unsuccessful.

I have added the torque controller to the fetch_bringup/config/default_controllers.yaml and restarted the robot service to load my controller. And from print debug statements, I can tell that I am able to stop the default follow_joint_trajectory and gravity_compensation controllers and start the torque controller.

However, the robot arm does not seem to respond to the setEffort commands within the controller. And so the arm remains stationary instead of tracking the trajectory.
I have been able to check that my controller works in Gazebo and that the simulated robot follows the torque commands, but cannot get the hardware to comply.

Are there some specific steps I have to follow or constraints I have to adhere to in order to send effort commands via setEffort from my controller?

Any and all comments are greatly appreciated!

p.s. the robot is running Ubuntu 14.04 if that information is helpful.

Eigen3 Warning on Kinetic

Warnings   << robot_controllers_interface:cmake /home/dave/ros/current/ws_moveit/logs/robot_controllers_interface/build.cmake.003.log                     
CMake Warning (dev) at CMakeLists.txt:45 (add_dependencies):
  Policy CMP0046 is not set: Error on non-existent dependency in
  add_dependencies.  Run "cmake --help-policy CMP0046" for policy details.
  Use the cmake_policy command to set the policy and suppress this warning.

  The dependency target "robot_controllers_msgs_gencpp" of target
  "robot_controllers_interface" does not exist.
This warning is for project developers.  Use -Wno-dev to suppress it.

Torso joint is suddenly going down

Hello,

I am trying to execute trajectories on both the arm and arm_torso group. I noticed something weird. When I am planning with arm_torso or arm individually, the robot is doing everything perfectly. But when I am changing the joint group to arm just after executing a trajectory on arm_torso, the robot suddenly drops its torso_lift_joint to '0'. It is happening with MoveIT, as well as executing custom trajectories. (If you just change the group to 'arm' from 'arm_torso' and execute any trajectory in MoveIT, the bug will show up)

I have seen this behaviour in Gazebo. I have a real Fetch robot, but I can not examine this on that (I really don't want to hurt it)

Please have a look. I will search further for solutions. Thank you

ROS2: add dynamic parameter interface to PID

In ROS2, parameters are naturally dynamic. We should use that to add the ability to reconfigure the PID class.

Add action for starting/stopping controllers

Cartesian controller for the Fetch arm using a joystick.

• debugging cartesian_twist.cpp.
• implementing two versions of twist controller for the twist commands based on torso_lift_link frame( body frame ) and wrist_roll_link frame( end-effector frame ).
• receiving twist commands signals from joystick with some key combinations.
• null space control ( optional ).