Moving motor

Source code

NOTE: Work in progress. We're continuously improving the documentation, stay tuned for updates.

This section will explain how to move MARA with a python script and C++.

#!/usr/bin/python3

# ROS 2.0
import rclpy
from rclpy.qos import qos_profile_default, qos_profile_sensor_data
# HRIM
from hrim_actuator_rotaryservo_msgs.msg import GoalRotaryServo

rclpy.init(args=None)

node = rclpy.create_node('test_finger_control_service')

publisher = node.create_publisher(GoalRotaryServo, '/hros_actuation_servomotor_000000000001/goal', qos_profile=qos_profile_sensor_data)

value = 90 # in degrees

msg = GoalRotaryServo()
msg.position = value * 3.1416/180
msg.velocity = 0.404
msg.control_type = 1
publisher.publish(msg)

rclpy.spin(node)

To run the script, you should source the ROS 2.0 env and then execute the script using python3:

python3 move_one_joint.py

When we program with Python, it is as simple as creating a script and running it. But in C++ we need to go to process of compilation. For this, we need to create the following directory structure:

| mara_minimal_publisher
| -- CMakeLists.txt
| -- package.xml
| -- src/
| -- src/mara_minimal_publisher.cpp

First thing to do, is to write the code to move one of the joints. We can set the quality of service (QoS) of a topic. ROS 2.0 provides QoS based on different use cases. The default one is accessible with rmw_qos_profile_default, and a more demanding QoS could be the one used for sensors or actuators: rmw_qos_profile_sensor_data.

It's important to initialize ROS 2.0 using rclcpp::init. Then we must create the node. In this case, the node is called mara_minimal_publisher. To be able to publish some data in the network, we should create a topic using the node of the requested type to set a goal in one of MARA's motors.

Finally, we create a new message, fill it with the requested goal (in this case we will move the motor using velocity and position) and use the topic to publish the data.

#include "rclcpp/rclcpp.hpp"
#include "hrim_actuator_rotaryservo_msgs/msg/goal_rotary_servo.hpp"

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);

  auto node = rclcpp::Node::make_shared("mara_minimal_publisher");

  // create a publisher
  auto joint_publisher = node->create_publisher<hrim_actuator_rotaryservo_msgs::msg::GoalRotaryServo>("/hros_actuation_servomotor_000000000001/goal", rmw_qos_profile_sensor_data);

  rclcpp::WallRate loop_rate(1);

  auto msg = std::make_shared<hrim_actuator_rotaryservo_msgs::msg::GoalRotaryServo>();

  float value = 90; // in degrees

  while (rclcpp::ok()) {
    msg->position = value * 3.1416/180.0;
    msg->velocity = 0.404;
    msg->control_type = 1;

    joint_publisher->publish(msg);

    loop_rate.sleep();
  }

  return 0;
}

For the compilation process, we need to create a package.xml which helps colcon or ament to understand which is the order in the compilation chain, among other matters.

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="2">
  <name>mara_minimal_publisher</name>
  <version>0.0.1</version>
  <description>MARA minimal publisher node</description>
  <maintainer email="alex@erlerobotics.com">Alejandro Hernandez</maintainer>
  <license>Apache License 2.0</license>
  <author>Alejandro Hernandez</author>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <build_depend>rclcpp</build_depend>
  <build_depend>hrim_actuator_rotaryservo_msgs</build_depend>

  <exec_depend>rclcpp</exec_depend>
  <exec_depend>hrim_actuator_rotaryservo_msgs</exec_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

CMakeLists.txt is the file that colcon is going to load to compile the sources of this project. It's important to include rclcpp and hrim_actuator_rotaryservo_msgs, which contain the messages:

cmake_minimum_required(VERSION 3.5)
project(mara_minimal_publisher)

# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(hrim_actuator_rotaryservo_msgs REQUIRED)

add_executable(mara_minimal_publisher src/mara_minimal_publisher.cpp)
ament_target_dependencies(mara_minimal_publisher rclcpp hrim_actuator_rotaryservo_msgs)

install(TARGETS
  mara_minimal_publisher
  DESTINATION lib/${PROJECT_NAME}
)

ament_package()