0
$\begingroup$

I am modifying existing hardware interface ros2_control_diff_drive(example_2) for my actual(physical) diff_drive setup. I have been following this tutorial.

In the example_2 files, i have modified only "diffbot_system.cpp" and "diffbot_system.hpp". NOTE THAT I HAVE NOT MODIFIED ANY URDF FILES.

I am getting the following error:

ros2_control_node-1] [INFO] [1730118567.236983125] [controller_manager]: Received robot description from topic.
[ros2_control_node-1] [INFO] [1730118567.238006594] [resource_manager]: Loading hardware 'DiffBot' 
[ros2_control_node-1] [ERROR] [1730118567.241820288] [controller_manager]: The published robot description file (urdf) seems not to be genuine. The following error was caught:open: No such file or directory
[spawner-3] [INFO] [1730118569.794680257] [spawner_diffbot_base_controller]: Waiting for '/controller_manager' services to be available
[spawner-3] [ERROR] [1730118577.901102661] [spawner_diffbot_base_controller]: Controller manager not available

The modified files are:

1. diffbot_system.hpp

// Copyright 2021 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef ROS2_CONTROL_DEMO_EXAMPLE_2__DIFFBOT_SYSTEM_HPP_
#define ROS2_CONTROL_DEMO_EXAMPLE_2__DIFFBOT_SYSTEM_HPP_

#include <memory>
#include <string>
#include <vector>

#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "rclcpp/clock.hpp"
#include "rclcpp/duration.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"

#include "ros2_control_demo_example_2/serial_comm.hpp"

namespace ros2_control_demo_example_2
{
class DiffBotSystemHardware : public hardware_interface::SystemInterface
{
public:
  RCLCPP_SHARED_PTR_DEFINITIONS(DiffBotSystemHardware);

  hardware_interface::CallbackReturn on_init(
    const hardware_interface::HardwareInfo & info) override;

  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;

  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;

  hardware_interface::CallbackReturn on_activate(
    const rclcpp_lifecycle::State & previous_state) override;

  hardware_interface::CallbackReturn on_deactivate(
    const rclcpp_lifecycle::State & previous_state) override;

  hardware_interface::return_type read(
    const rclcpp::Time & time, const rclcpp::Duration & period) override;

  hardware_interface::return_type write(
    const rclcpp::Time & time, const rclcpp::Duration & period) override;

private:

  GlowLed led_1;

  // Store the command for the simulated robot
  std::vector<double> hw_commands_;
  std::vector<double> hw_positions_;
  std::vector<double> hw_velocities_;
};

}  // namespace ros2_control_demo_example_2

#endif  // ROS2_CONTROL_DEMO_EXAMPLE_2__DIFFBOT_SYSTEM_HPP_

2. diffbot_system.cpp

// Copyright 2021 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "ros2_control_demo_example_2/diffbot_system.hpp"

#include <chrono>
#include <cmath>
#include <cstddef>
#include <limits>
#include <memory>
#include <vector>
//#include <string>

#include "hardware_interface/lexical_casts.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/rclcpp.hpp"

namespace ros2_control_demo_example_2
{
hardware_interface::CallbackReturn DiffBotSystemHardware::on_init(
  const hardware_interface::HardwareInfo & info)
{
  if (
    hardware_interface::SystemInterface::on_init(info) !=
    hardware_interface::CallbackReturn::SUCCESS)
  {
    return hardware_interface::CallbackReturn::ERROR;
  }

  hw_positions_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
  hw_velocities_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
  hw_commands_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());

  for (const hardware_interface::ComponentInfo & joint : info_.joints)
  {
    // DiffBotSystem has exactly two states and one command interface on each joint
    if (joint.command_interfaces.size() != 1)
    {
      RCLCPP_FATAL(
        rclcpp::get_logger("DiffBotSystemHardware"),
        "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(),
        joint.command_interfaces.size());
      return hardware_interface::CallbackReturn::ERROR;
    }

    if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY)
    {
      RCLCPP_FATAL(
        rclcpp::get_logger("DiffBotSystemHardware"),
        "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(),
        joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY);
      return hardware_interface::CallbackReturn::ERROR;
    }

    if (joint.state_interfaces.size() != 2)
    {
      RCLCPP_FATAL(
        rclcpp::get_logger("DiffBotSystemHardware"),
        "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(),
        joint.state_interfaces.size());
      return hardware_interface::CallbackReturn::ERROR;
    }

    if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION)
    {
      RCLCPP_FATAL(
        rclcpp::get_logger("DiffBotSystemHardware"),
        "Joint '%s' have '%s' as first state interface. '%s' expected.", joint.name.c_str(),
        joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
      return hardware_interface::CallbackReturn::ERROR;
    }

    if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY)
    {
      RCLCPP_FATAL(
        rclcpp::get_logger("DiffBotSystemHardware"),
        "Joint '%s' have '%s' as second state interface. '%s' expected.", joint.name.c_str(),
        joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY);
      return hardware_interface::CallbackReturn::ERROR;
    }
  }

  return hardware_interface::CallbackReturn::SUCCESS;
}

std::vector<hardware_interface::StateInterface> DiffBotSystemHardware::export_state_interfaces()
{
  std::vector<hardware_interface::StateInterface> state_interfaces;
  for (auto i = 0u; i < info_.joints.size(); i++)
  {
    state_interfaces.emplace_back(hardware_interface::StateInterface(
      info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_[i]));
    state_interfaces.emplace_back(hardware_interface::StateInterface(
      info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_[i]));
  }

  return state_interfaces;
}

std::vector<hardware_interface::CommandInterface> DiffBotSystemHardware::export_command_interfaces()
{
  std::vector<hardware_interface::CommandInterface> command_interfaces;
  for (auto i = 0u; i < info_.joints.size(); i++)
  {
    command_interfaces.emplace_back(hardware_interface::CommandInterface(
      info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i]));
  }

  return command_interfaces;
}

hardware_interface::CallbackReturn DiffBotSystemHardware::on_activate(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Activating ...please wait...");

  // set some default values
  for (auto i = 0u; i < hw_positions_.size(); i++)
  {
    if (std::isnan(hw_positions_[i]))
    {
      hw_positions_[i] = 0;
      hw_velocities_[i] = 0;
      hw_commands_[i] = 0;
    }
  }

  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn DiffBotSystemHardware::on_deactivate(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Deactivating...");  

  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::return_type DiffBotSystemHardware::read(
  const rclcpp::Time & /*time*/, const rclcpp::Duration & period)
{
  RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Reading...");
  return hardware_interface::return_type::OK;
}

hardware_interface::return_type ros2_control_demo_example_2 ::DiffBotSystemHardware::write(
  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
{
  RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Writing: %.5f", hw_commands_[0]);

  // for (auto i = 0u; i < hw_commands_.size(); i++)

  led_1.send_data(std::to_string(hw_commands_[0]));

  return hardware_interface::return_type::OK;
}

}  // namespace ros2_control_demo_example_2

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(
  ros2_control_demo_example_2::DiffBotSystemHardware, hardware_interface::SystemInterface)

Let me know if any other information is required.

Edit_1: After further inspection, I found that an error occurs specifically when creating an object of the class GlowLed in the file diffbot_system.hpp, under the Private tag, using GlowLed led_1_;.

Below is the code for "serial_comm.hpp" containing definition of class "GlowLed".

3. serial_comm.hpp

#ifndef DIFFDRIVE_ARDUINO_ARDUINO_COMMS_HPP

#define DIFFDRIVE_ARDUINO_ARDUINO_COMMS_HPP

#include #include <boost/asio.hpp> #include

class GlowLed { public: GlowLed():serial(io, port){ port = "/dev/ttyACM0"; baud_rate = 9600;

        serial.set_option(boost::asio::serial_port_base::baud_rate(baud_rate));
    } 

    void send_data(std::string data){

                // std::cout<<"Enter 1 to turn LED ON and 0 to turn LED OFF.";
            
                boost::asio::write(serial, boost::asio::buffer(data));
                
                //std::this_thread::sleep_for(std::chrono::seconds(1));

            
    }

private:
    std::string port;
    unsigned int baud_rate;
    boost::asio::io_service io;
    boost::asio::serial_port serial;
    // std::string data;

};

#endif

Thanks

Improve this question
$\endgroup$
3
  • $\begingroup$ have you used the correct branch (e.g., humble for humble distro?). Have you built and sourced the ros2_control_demo_description package? use colcon build --packages-up-to ros2_control_demo_example_2 $\endgroup$
    – Christoph Froehlich
    Commented Oct 30 at 9:27
  • $\begingroup$ I cloned the demo repo locally and switched to Iron branch using git. Next i took only example_2 folder and discarded the rest of files/folders then build it using colcon build. NOTE that before making any changes to "diffbot_system.cpp" and "diffbot_system.hpp" file, every thing was working correctly. Also even if I now change the content of these 2 files back to initial one, the code works. $\endgroup$
    – Pratham
    Commented Oct 30 at 15:31
  • $\begingroup$ @ChristophFroehlich I found that an error occurs specifically when creating an object of the class "GlowLed" in the file "diffbot_system.hpp". "GlowLed" class is created to communicate with hardware. Check "Edit_1" part of the question for more information. $\endgroup$
    – Pratham
    Commented Oct 31 at 16:45

1 Answer 1

Reset to default
0
$\begingroup$

The issue was not at all related to URDF.
The issue was with serial_comm.hpp. The issue with the order of initialization in the GlowLed constructor, specifically in the following line:

GlowLed():serial(io, port){

serial(io, port) tries to initialize the serial object with port and io, but at this point, port is still uninitialized.

The corrected code is: -->serial_comm.hpp

#ifndef SERIAL_COMM_HPP
#define SERIAL_COMM_HPP

#include <iostream>
#include <boost/asio.hpp>
#include <thread>

class GlowLed
{
public:
    GlowLed() : port("/dev/ttyACM0"), baud_rate(9600), serial(io, port)
    {
        // Now, initialize the serial port options
        serial.set_option(boost::asio::serial_port_base::baud_rate(baud_rate));
    }

    void send_data(std::string data)
    {
        boost::asio::write(serial, boost::asio::buffer(data));
    }

private:
    std::string port;
    unsigned int baud_rate;
    boost::asio::io_service io;
    boost::asio::serial_port serial;
};

#endif

NOTE: Make sure the device(raspsberry pi pico in my case) is connected to pc/laptop. In my case, i am running ros2 on laptop and sending hardware command to pico via serial to glow led.

Improve this answer
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.