4
\$\begingroup\$

This code is for a ROS (http://www.ros.org/) node that controls a pair of servo motors (referred to in the code as grippers) that can be controlled individually or together. When they are controlled together they move in opposite directions to create a gripping motion.

Running this code requires both ROS and the Dynamixel SDK which is a tall order so I'm mostly looking for general advice on best practice, efficiency, or any potential issues that can be spotted.

Most of the code for directly interacting with the port was taken from the SDK examples. This is pretty long, I don't know if that's an issue.

The node file, manipulator.cpp:

#include "../include/manipulator/manipulator.h"

Grippers grippers;

//actuator callbacks
void grDirLeftCallback(const std_msgs::Float32::ConstPtr &msg)
{
    grippers.setDesired(LEFT_GRIPPER, msg->data);
}
void grDirRightCallback(const std_msgs::Float32::ConstPtr &msg)
{
    grippers.setDesired(RIGHT_GRIPPER, msg->data);
}
//topic "au_gDesiredJoint" moves both grippers to symmetrical points around the center
void grDirJointCallback(const std_msgs::Float32::ConstPtr &msg)
{
    //assumes both grippers move ccw with increasing value
    grippers.setDesired(LEFT_GRIPPER, (grippers.getMin(LEFT_GRIPPER) + msg->data + grippers.getOffset(LEFT_GRIPPER)));
    grippers.setDesired(RIGHT_GRIPPER, (grippers.getMax(RIGHT_GRIPPER) - msg->data + grippers.getOffset(RIGHT_GRIPPER)));
}
void dirArmCallback(const std_msgs::Float32::ConstPtr &msg)
{
    grippers.setDesired(ARM, msg->data);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "manipulator_node");

    ros::NodeHandle n;

    grippers.setup();
    grippers.setSpeed(LEFT_GRIPPER,DESIRED_SPEED);
    grippers.setSpeed(RIGHT_GRIPPER,DESIRED_SPEED);

    //publishers to send out acutator positions
    std_msgs::Float32 curLeftPub;
    ros::Publisher topicCurrentLeft = n.advertise<std_msgs::Float32>("au_gCurrentLeft", 100);
    std_msgs::Float32 curRightPub;
    ros::Publisher topicCurrentRight = n.advertise<std_msgs::Float32>("au_gCurrentRight", 100);
    std_msgs::Float32 curArmPub;
    ros::Publisher topicCurrentArm = n.advertise<std_msgs::Float32>("au_currentArm", 100);

    //subscribers for desired positions
    //only want to keep the most recent desired position
    ros::Subscriber topicGrDirLeft = n.subscribe("au_gDesiredLeft", 1, grDirLeftCallback);
    ros::Subscriber topicGrDirRight = n.subscribe("au_gDesiredRight", 1, grDirRightCallback);
    ros::Subscriber topicGrDirJoin = n.subscribe("au_gDesiredJoint", 1, grDirJointCallback);
    ros::Subscriber topicDirArm = n.subscribe("au_desiredArm", 1, dirArmCallback);

    ros::Rate loop_rate(10);

    while (ros::ok())
    {
        grippers.run();
        grippers.read();

        curLeftPub.data = grippers.getCurrent(LEFT_GRIPPER);
        curRightPub.data = grippers.getCurrent(RIGHT_GRIPPER);

        topicCurrentLeft.publish(curLeftPub);
        topicCurrentRight.publish(curRightPub);

        ros::spinOnce();

        loop_rate.sleep();
    }

    grippers.shutdown();

    return 0;
}

manipulator.h:

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Float32.h"
#include "../include/dynamixel_sdk/dynamixel_sdk.h"
#include "grippers.h"

grippers.h

#ifndef GRIPPERS_H
#define GRIPPERS_H

#include "../include/dynamixel_sdk/dynamixel_sdk.h"

//this node already contains some code for expansion when the manipulator is replaced with a dynamixel motor
//only the grippers are implmented currently

#define NUM_DEVICES 4 //actually only 3 but dynamixel ids are 1 indexed and I want the numbers to line up. min/max/dir[0] is unused
#define LEFT_GRIPPER 1
#define RIGHT_GRIPPER 2
#define ARM 3
#define NOT_IN_USE -1

// Control table address for MX-64-AT
#define ADDR_MX_TORQUE_ENABLE 24
#define ADDR_MX_GOAL_POSITION 30
#define ADDR_MX_MOVING_SPEED 32
#define ADDR_MX_PRESENT_POSITION 36

// Data Byte Length
#define LEN_MX_GOAL_POSITION 2
#define LEN_MX_PRESENT_POSITION 2

// Protocol version
#define PROTOCOL_VERSION 1.0 // See which protocol version is used in the Dynamixel

// Default setting
#define BAUDRATE 57600
#define DEVICENAME "/dev/ttyUSB0" // Check which port is being used on your controller

#define TORQUE_ENABLE 1                // Value for enabling the torque
#define TORQUE_DISABLE 0               // Value for disabling the torque
#define DXL_MOVING_STATUS_THRESHOLD 10 // Dynamixel moving status threshold
#define DEGREES_PER_STEP 0.088
#define DESIRED_SPEED 100

class Grippers
{
    float min[NUM_DEVICES] = {NOT_IN_USE,165,40,0};
    float max[NUM_DEVICES] = {NOT_IN_USE,185,60,360};

    float current[NUM_DEVICES] = {0}; //current position in degrees

    uint16_t dir[NUM_DEVICES] = {0};  //goal position in steps

    bool newDirPos = false;
    bool move[NUM_DEVICES] = {false};

    dynamixel::PortHandler *portHandler;
    dynamixel::PacketHandler *packetHandler;
    dynamixel::GroupSyncWrite groupSyncWrite;

    float offsetLeft = 0;
    float offsetRight = 0;

    uint8_t dxl_error = 0;
    uint8_t param_goal_position[2] = {0};
    uint16_t dxl1_present_position = 0, dxl2_present_position = 0;


    int dxl_comm_result = COMM_TX_FAIL; // Communication result
    bool dxl_addparam_result = false;   // addParam result

public:
    Grippers();
    ~Grippers();
    void setDesired(int, float);
    bool setup();
    void shutdown();
    void run();
    void read();
    float getCurrent(int);
    float getDesired(int);
    void setSpeed(int,int);
    float getMin(int);
    float getMax(int);
    float getOffset(int);

};

#endif // GRIPPERS_H

grippers.cpp

#include "../include/manipulator/grippers.h"

Grippers::Grippers()
    : portHandler(dynamixel::PortHandler::getPortHandler(DEVICENAME)),
      packetHandler(dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION)),
      groupSyncWrite(portHandler, packetHandler, ADDR_MX_GOAL_POSITION, LEN_MX_GOAL_POSITION)


{

}

Grippers::~Grippers()
{
    delete portHandler;
    delete packetHandler;
}

void Grippers::setDesired(int device, float value)
{
    float desiredDegrees;
    if (value > max[device])
    {
        desiredDegrees = max[device];
    }
    else if (value < min[device])
    {
        desiredDegrees = min[device];
    }
    else
    {
        desiredDegrees = value;
    }
    dir[device] = desiredDegrees / DEGREES_PER_STEP; //converts angles to steps
    move[device] = true;
    newDirPos = true; //flag so that communication only occurs once per new command
}

void Grippers::run()
{
    if (move[LEFT_GRIPPER])
    {
        // Allocate goal position value into byte array
        param_goal_position[0] = DXL_LOBYTE(dir[LEFT_GRIPPER]);
        param_goal_position[1] = DXL_HIBYTE(dir[LEFT_GRIPPER]);

        // Add Dynamixel#1 goal position value to the Syncwrite storage
        dxl_addparam_result = groupSyncWrite.addParam(LEFT_GRIPPER, param_goal_position);
        if (dxl_addparam_result != true)
        {
            fprintf(stderr, "[ID:%03d] groupSyncWrite addparam failed", LEFT_GRIPPER);
        }
        move[LEFT_GRIPPER] = false;
    }
    if (move[RIGHT_GRIPPER])
    {
        param_goal_position[0] = DXL_LOBYTE(dir[RIGHT_GRIPPER]);
        param_goal_position[1] = DXL_HIBYTE(dir[RIGHT_GRIPPER]);

        // Add Dynamixel#2 goal position value to the Syncwrite parameter storage
        dxl_addparam_result = groupSyncWrite.addParam(RIGHT_GRIPPER, param_goal_position);
        if (dxl_addparam_result != true)
        {
            fprintf(stderr, "[ID:%03d] groupSyncWrite addparam failed", RIGHT_GRIPPER);
        }
        move[RIGHT_GRIPPER] = false;
    }
    //only write the move command once per received goal
    if (newDirPos)
    {
        // Syncwrite goal position
        dxl_comm_result = groupSyncWrite.txPacket();
        if (dxl_comm_result != COMM_SUCCESS)
            printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));

        // Clear syncwrite parameter storage
        groupSyncWrite.clearParam();
        newDirPos = false;
    }

}

void Grippers::setSpeed(int device, int speed)
{
    dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, device, ADDR_MX_MOVING_SPEED, speed, &dxl_error);
    if (dxl_comm_result != COMM_SUCCESS)
    {
        printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
    }
    else if (dxl_error != 0)
    {
        printf("%s\n", packetHandler->getRxPacketError(dxl_error));
    }
    else
    {
        printf("speed set to %d\n", speed);
    }

}

void Grippers::read()
{
    // Read left gripper present position
    dxl_comm_result = packetHandler->read2ByteTxRx(portHandler, LEFT_GRIPPER, ADDR_MX_PRESENT_POSITION, &dxl1_present_position, &dxl_error);
    if (dxl_comm_result != COMM_SUCCESS)
    {
        printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
    }
    else if (dxl_error != 0)
    {
        printf("%s\n", packetHandler->getRxPacketError(dxl_error));
    }

    // Read right gripper present position
    dxl_comm_result = packetHandler->read2ByteTxRx(portHandler, RIGHT_GRIPPER, ADDR_MX_PRESENT_POSITION, &dxl2_present_position, &dxl_error);
    if (dxl_comm_result != COMM_SUCCESS)
    {
        printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
    }
    else if (dxl_error != 0)
    {
        printf("%s\n", packetHandler->getRxPacketError(dxl_error));
    }

    //printf("[ID:%03d] GoalPos:%03d  PresPos:%03d\t[ID:%03d] GoalPos:%03d  PresPos:%03d\n", LEFT_GRIPPER, dir[LEFT_GRIPPER], dxl1_present_position, RIGHT_GRIPPER, dir[RIGHT_GRIPPER], dxl1_present_position);

    current[LEFT_GRIPPER] = dxl1_present_position * DEGREES_PER_STEP;
    current[RIGHT_GRIPPER] = dxl2_present_position * DEGREES_PER_STEP;
}

void Grippers::shutdown()
{
    printf("shutting down port...\n");
    // on node shut down turn off motor torque and close the connection
    // Disable left gripper Torque
    dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, LEFT_GRIPPER, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE, &dxl_error);
    if (dxl_comm_result != COMM_SUCCESS)
    {
        printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
    }
    else if (dxl_error != 0)
    {
        printf("%s\n", packetHandler->getRxPacketError(dxl_error));
    }
    else
    {
        printf("turned off dxl1 torque\n");
    }

    // Disable right gripper Torque
    dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, RIGHT_GRIPPER, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE, &dxl_error);
    if (dxl_comm_result != COMM_SUCCESS)
    {
        printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
    }
    else if (dxl_error != 0)
    {
        printf("%s\n", packetHandler->getRxPacketError(dxl_error));
    }
    else
    {
        printf("turned off dxl2 torque\n");
    }

    // Close port
    portHandler->closePort();
    printf("port closed\n");
}

bool Grippers::setup()
{
    if (portHandler->openPort())
    {
        printf("Succeeded to open the port!\n");
    }
    else
    {
        printf("Failed to open the port!\n");
        return 0;
    }

    // Set port baudrate
    if (portHandler->setBaudRate(BAUDRATE))
    {
        printf("Succeeded to change the baudrate!\n");
    }
    else
    {
        printf("Failed to change the baudrate!\n");
        return 0;
    }

    // Enable Dynamixel#1 Torque
    dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, LEFT_GRIPPER, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
    if (dxl_comm_result != COMM_SUCCESS)
    {
        printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
    }
    else if (dxl_error != 0)
    {
        printf("%s\n", packetHandler->getRxPacketError(dxl_error));
    }
    else
    {
        printf("Dynamixel#%d has been successfully connected \n", LEFT_GRIPPER);
    }

    // Enable Dynamixel#2 Torque
    dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, RIGHT_GRIPPER, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
    if (dxl_comm_result != COMM_SUCCESS)
    {
        printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
    }
    else if (dxl_error != 0)
    {
        printf("%s\n", packetHandler->getRxPacketError(dxl_error));
    }
    else
    {
        printf("Dynamixel#%d has been successfully connected \n", RIGHT_GRIPPER);
    }
    return true;
}

float Grippers::getCurrent(int device)
{
    return current[device];
}

float Grippers::getDesired(int device)
{
    return dir[device];
}

float Grippers::getMin(int device)
{
    return min[device];
}

float Grippers::getMax(int device)
{
    return max[device];
}

float Grippers::getOffset(int device)
{
    return (device == 1) ? offsetLeft : offsetRight;
}

Any advice would be appreciated, if anything doesn't make sense I'll try to explain.

\$\endgroup\$
3
\$\begingroup\$

Welcome to Code Review. First of all, I would like to say: embrace C++ fully, not partially.

  • use std::unique_ptr where appropriate - portHandler and packetHandler
  • DRY - do not repeat yourself - void Grippers::run() - if (move[LEFT_GRIPPER]) ... if (move[RIGHT_GRIPPER]) ... - same code, make a method for the common part.
  • use constexpr, enum, or enum class instead of #define, leave #define to where you really need conditional compilation - #if (or #ifdef but undefined symbols are considered zero, at least by IAR compiler)
  • inline simple getters and setters - Grippers::getCurrent etc. - move the body to the header (I prefer inline body - inside the class - over under the class), this actually helps a lot in embedded systems, where you should be more careful about speed (unless you are out of space but inlined getters are still a win for both speed and size).
  • consider broader usage, maybe even templates (e.g. template<size_t NumDevices>) - Grippers::Grippers() should, in my opinion, take arguments (deviceName and protocolVersion) with possibly default values, instead of those #define's (or use global constexpr if you want some global configuration header - which should probably be separated to e.g. grippers_config.h being included into grippers.h or maybe some global configuration.h with constants to be used when creating the Grippers class, which need not to know the configuration but should be a bit more universal).
  • document your code - it will not only help your older self, or possible successor, but also often helps to make your mind, get different perspective, think it through in a bit different way - because you have to explain it.
  • don't be lazy to create reusable helpers instead of using hacks like leaving first element of an array empty - create something like std::array with 1-based indexing! You will, hopefully, find that you actually need to be lazy enough to help yourself by creating helpers and automate things not to write something over and over again ;) Be lazy in proper way :D
  • #include "../include/... well, I would use compiler option for include path to strip that off (-I../include)
\$\endgroup\$
  • \$\begingroup\$ Thank you so much for the help. I'm trying to use std::unique_ptr for the pointers but the object groupSyncWrite call shows an error that says no constructor expects std::unique_ptr. Aside from editing the library is there a workaround for this? \$\endgroup\$ – Ben H Nov 7 '18 at 21:40
  • 1
    \$\begingroup\$ @BenH: unique_ptr::get - you would have to explicitly change all non-owning pointer transwer with this. \$\endgroup\$ – user52292 Nov 7 '18 at 21:58
2
\$\begingroup\$

General observation

  • Pack all your code into a personal namespace , you have no reason to don't do it.
  • Modify your includes directory to point also to ../include/ so you'll just have to write #include "dynamixel_sdk/dynamixel_sdk.h" and #include "manipulator/grippers.h"

File grippers.h

  • You define way too much preprocessor contants, use static const values instead, or even better, use static constexpr since you use c++11. If a group of constants are related kinds, regroup them into class enums , that will add information about their relationship. (more: why not macro [1], constexpr vs macro [2], constexpr vs const [3], static const vs #define [4], static constexpr vs (old) enum [5] and The CppCoreGuidelines [6]).
  • Don't forget to #include <cstdint> to handling
  • Be careful, you assign integer to float, prefer format them as float min = 164.f;
  • Instead of having five variables, as array, holding each one aspect of each devices, you could define a struc device holding together all aspect of one only device . After, since you know the size of your array at compile time, you should using std::array instead of plain old array to get something like:

    #include <cstdint>
    #include <array>
    
    //...
    
    namespace device {
        // can't use enum class because you use it as indexes.
        enum id {
            left_gripper = 1,
            right_gripper,
            arm,
            max // num_devices
        };
        struct entity {
            float    min{0.f};
            float    max{0.f};
            float    position{0};
            uint16_t direction{0};
            bool     move{false};
        };
    };
    // ...
    // in class Grippers declaration:
    std::array<device::entity, device::max> devices = {{
        {not_in_use, not_in_use, 0u, 0u, false},    
        {165.f, 185.f, 0u, 0u, false},
        {40.f, 60.f, 0u, 0u, false},
        {0.f, 360.f, 0u, 0u, false}
    }};
    

and for accessing devices you use something like: devices[device::left_gripper].max = ...

  • Maybe try to use std::unique_ptr instead old raw pointers, many advantages, for almost no drawback.
  • The member param_goal_position is an array of two integers. In other words, a pair of integers, why not using a std::pair<uint8_t, uint8_t> ?
  • Don't omit parameter names in your prototype declarations. It make your code less readable and your intentions less clear. It gets a lot easier to remember how the code work, looking the header file, with prototypes speaking by himself.
  • IMHO, it's better to put public members and functions first, then, the protected ones if there are, and finally the private ones. That's because, what's matter is the public interface of your class, it's easier if people (and you included) can see directly the interface.

File grippers.cpp

  • You make a large use of printf(...) and fprintf(...) but didn't writes #include <stdio>. Even if it work as it (surely dynamixel_sdk.h include it somewhere), it's better if you explicitly list every required headers.

  • You can simplify the if...else in the function setDesired using std::clamp that is only available from c++17but basically, it's implemented as:

    template<class T>
    constexpr const T& clamp( const T& value, const T& lower, const T& hight)
    {
        return (value < lower) ? lower : comp(hight < value) ? hight : value;
    }
    
  • Try to factorize code, as stated @firda, DRY ! If you don't want to write a function just to regroup repetitions from two first branches in Grippers::run, at least, make it a lambda.

\$\endgroup\$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

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