Skip to content

Commit

Permalink
Guard handle usage with shared mutex
Browse files Browse the repository at this point in the history 
#181
  • Loading branch information
@PeterBowman
PeterBowman committed Feb 10, 2022
1 parent bd153db commit 1fb36fc
Show file tree
Hide file tree
Showing 8 changed files with 53 additions and 43 deletions.
3 changes: 2 additions & 1 deletion libraries/YarpPlugins/AmorControlboard/AmorControlboard.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
#ifndef __AMOR_CONTROLBOARD_HPP__
#define __AMOR_CONTROLBOARD_HPP__

#include <stdio.h>
#include <mutex>
#include <vector>

#include <yarp/os/all.h>
Expand Down Expand Up @@ -653,6 +653,7 @@ class AmorControlboard : public yarp::dev::DeviceDriver,
private:

AMOR_HANDLE handle;
mutable std::mutex handleMutex;
yarp::dev::PolyDriver cartesianControllerDevice;
bool usingCartesianController;
int controlMode;
Expand Down
2 changes: 2 additions & 0 deletions libraries/YarpPlugins/AmorControlboard/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,8 @@ if(NOT SKIP_AmorControlboard)
YARP::YARP_dev
AMOR::amor_api)

target_compile_features(AmorControlboard PRIVATE cxx_std_17)

yarp_install(TARGETS AmorControlboard
LIBRARY DESTINATION ${ROBOTICSLAB-YARP-DEVICES_DYNAMIC_PLUGINS_INSTALL_DIR}
ARCHIVE DESTINATION ${ROBOTICSLAB-YARP-DEVICES_STATIC_PLUGINS_INSTALL_DIR}
Expand Down
9 changes: 3 additions & 6 deletions libraries/YarpPlugins/AmorControlboard/DeviceDriverImpl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,14 +47,12 @@ bool AmorControlboard::open(yarp::os::Searchable& config)
if (amor_get_joint_info(handle, j, &jointInfo[j]) != AMOR_SUCCESS)
{
yCError(AMOR) << "amor_get_joint_info() failed for joint" << j << "with error:" << amor_error();
amor_release(handle);
return false;
}

if (amor_get_status(handle, j, &jointStatus[j]) != AMOR_SUCCESS)
{
yCError(AMOR) << "amor_get_status() failed for joint" << j << "with error:" << amor_error();
amor_release(handle);
return false;
}
}
Expand All @@ -64,7 +62,6 @@ bool AmorControlboard::open(yarp::os::Searchable& config)
if (!getEncoders(positions.data()))
{
yCError(AMOR) << "getEncoders() failed";
amor_release(handle);
return false;
}

Expand All @@ -74,7 +71,6 @@ bool AmorControlboard::open(yarp::os::Searchable& config)
if (!positionMove(positions.data()))
{
yCError(AMOR) << "positionMove() failed";
amor_release(handle);
return false;
}

Expand All @@ -87,22 +83,23 @@ bool AmorControlboard::open(yarp::os::Searchable& config)
usingCartesianController = true;

std::string subdevice = "AmorCartesianControl";
yarp::os::Value vHandle(&handle, sizeof handle);
yarp::os::Value vHandle(&handle, sizeof(handle));
yarp::os::Value vHandleMutex(&handleMutex, sizeof(handleMutex));
yarp::os::Property cartesianControllerOptions;

cartesianControllerOptions.fromString((config.toString()));
cartesianControllerOptions.put("device", "CartesianControlServer");
cartesianControllerOptions.put("subdevice", subdevice);
cartesianControllerOptions.put("name", cartesianControllerName->asString());
cartesianControllerOptions.put("handle", vHandle);
cartesianControllerOptions.put("handleMutex", vHandleMutex);
cartesianControllerOptions.setMonitor(config.getMonitor(), subdevice.c_str());

cartesianControllerDevice.open(cartesianControllerOptions);

if (!cartesianControllerDevice.isValid())
{
yCError(AMOR) << "AMOR cartesian controller device not valid";
amor_release(handle);
return false;
}
}
Expand Down
4 changes: 2 additions & 2 deletions libraries/YarpPlugins/AmorControlboard/IControlLimitsImpl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ bool AmorControlboard::getLimits(int axis, double *min, double *max)

AMOR_JOINT_INFO parameters;

if (amor_get_joint_info(handle, axis, &parameters) != AMOR_SUCCESS)
if (std::lock_guard<std::mutex> lock(handleMutex); amor_get_joint_info(handle, axis, &parameters) != AMOR_SUCCESS)
{
yCError(AMOR, "amor_get_joint_info() failed: %s", amor_error());
return false;
Expand Down Expand Up @@ -70,7 +70,7 @@ bool AmorControlboard::getVelLimits(int axis, double *min, double *max)

AMOR_JOINT_INFO parameters;

if (amor_get_joint_info(handle, axis, &parameters) != AMOR_SUCCESS)
if (std::lock_guard<std::mutex> lock(handleMutex); amor_get_joint_info(handle, axis, &parameters) != AMOR_SUCCESS)
{
yCError(AMOR, "amor_get_joint_info() failed: %s", amor_error());
return false;
Expand Down
20 changes: 10 additions & 10 deletions libraries/YarpPlugins/AmorControlboard/ICurrentControlImpl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@ bool AmorControlboard::getCurrent(int m, double *curr)

AMOR_VECTOR7 currents;

if (amor_get_actual_currents(handle, &currents) != AMOR_SUCCESS)
if (std::lock_guard<std::mutex> lock(handleMutex); amor_get_actual_currents(handle, &currents) != AMOR_SUCCESS)
{
yCError(AMOR, "amor_get_actual_currents() failed: %s", amor_error());
return false;
Expand All @@ -49,7 +49,7 @@ bool AmorControlboard::getCurrents(double *currs)

AMOR_VECTOR7 currents;

if (amor_get_actual_currents(handle, &currents) != AMOR_SUCCESS)
if (std::lock_guard<std::mutex> lock(handleMutex); amor_get_actual_currents(handle, &currents) != AMOR_SUCCESS)
{
yCError(AMOR, "amor_get_actual_currents() failed: %s", amor_error());
return false;
Expand All @@ -73,7 +73,7 @@ bool AmorControlboard::getCurrentRange(int m, double *min, double *max)

AMOR_JOINT_INFO parameters;

if (amor_get_joint_info(handle, m, &parameters) != AMOR_SUCCESS)
if (std::lock_guard<std::mutex> lock(handleMutex); amor_get_joint_info(handle, m, &parameters) != AMOR_SUCCESS)
{
yCError(AMOR, "amor_get_joint_info() failed: %s", amor_error());
return false;
Expand Down Expand Up @@ -111,7 +111,7 @@ bool AmorControlboard::setRefCurrents(const double *currs)

std::copy(currs, currs + AMOR_NUM_JOINTS, currents);

if (amor_set_currents(handle, currents) != AMOR_SUCCESS)
if (std::lock_guard<std::mutex> lock(handleMutex); amor_set_currents(handle, currents) != AMOR_SUCCESS)
{
yCError(AMOR, "amor_set_currents() failed: %s", amor_error());
return false;
Expand All @@ -133,15 +133,15 @@ bool AmorControlboard::setRefCurrent(int m, double curr)

AMOR_VECTOR7 currents;

if (amor_get_actual_currents(handle, &currents) != AMOR_SUCCESS)
if (std::lock_guard<std::mutex> lock(handleMutex); amor_get_actual_currents(handle, &currents) != AMOR_SUCCESS)
{
yCError(AMOR, "amor_get_actual_currents() failed: %s", amor_error());
return false;
}

currents[m] = curr;

if (amor_set_currents(handle, currents) != AMOR_SUCCESS)
if (std::lock_guard<std::mutex> lock(handleMutex); amor_set_currents(handle, currents) != AMOR_SUCCESS)
{
yCError(AMOR, "amor_set_currents() failed: %s", amor_error());
return false;
Expand All @@ -158,7 +158,7 @@ bool AmorControlboard::setRefCurrents(const int n_motor, const int *motors, cons

AMOR_VECTOR7 currents;

if (amor_get_actual_currents(handle, &currents) != AMOR_SUCCESS)
if (std::lock_guard<std::mutex> lock(handleMutex); amor_get_actual_currents(handle, &currents) != AMOR_SUCCESS)
{
yCError(AMOR, "amor_get_actual_currents() failed: %s", amor_error());
return false;
Expand All @@ -169,7 +169,7 @@ bool AmorControlboard::setRefCurrents(const int n_motor, const int *motors, cons
currents[motors[i]] = currs[i];
}

if (amor_set_currents(handle, currents) != AMOR_SUCCESS)
if (std::lock_guard<std::mutex> lock(handleMutex); amor_set_currents(handle, currents) != AMOR_SUCCESS)
{
yCError(AMOR, "amor_set_currents() failed: %s", amor_error());
return false;
Expand All @@ -186,7 +186,7 @@ bool AmorControlboard::getRefCurrents(double *currs)

AMOR_VECTOR7 currents;

if (amor_get_req_currents(handle, &currents) != AMOR_SUCCESS)
if (std::lock_guard<std::mutex> lock(handleMutex); amor_get_req_currents(handle, &currents) != AMOR_SUCCESS)
{
yCError(AMOR, "amor_get_req_currents() failed: %s", amor_error());
return false;
Expand All @@ -210,7 +210,7 @@ bool AmorControlboard::getRefCurrent(int m, double *curr)

AMOR_VECTOR7 currents;

if (amor_get_req_currents(handle, &currents) != AMOR_SUCCESS)
if (std::lock_guard<std::mutex> lock(handleMutex); amor_get_req_currents(handle, &currents) != AMOR_SUCCESS)
{
yCError(AMOR, "amor_get_req_currents() failed: %s", amor_error());
return false;
Expand Down
8 changes: 4 additions & 4 deletions libraries/YarpPlugins/AmorControlboard/IEncodersImpl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ bool AmorControlboard::getEncoder(int j, double *v)

AMOR_VECTOR7 positions;

if (amor_get_actual_positions(handle, &positions) != AMOR_SUCCESS)
if (std::lock_guard<std::mutex> lock(handleMutex); amor_get_actual_positions(handle, &positions) != AMOR_SUCCESS)
{
yCError(AMOR, "amor_get_actual_positions() failed: %s", amor_error());
return false;
Expand All @@ -72,7 +72,7 @@ bool AmorControlboard::getEncoders(double *encs)

AMOR_VECTOR7 positions;

if (amor_get_actual_positions(handle, &positions) != AMOR_SUCCESS)
if (std::lock_guard<std::mutex> lock(handleMutex); amor_get_actual_positions(handle, &positions) != AMOR_SUCCESS)
{
yCError(AMOR, "amor_get_actual_positions() failed: %s", amor_error());
return false;
Expand All @@ -99,7 +99,7 @@ bool AmorControlboard::getEncoderSpeed(int j, double *sp)

AMOR_VECTOR7 velocities;

if (amor_get_actual_velocities(handle, &velocities) != AMOR_SUCCESS)
if (std::lock_guard<std::mutex> lock(handleMutex); amor_get_actual_velocities(handle, &velocities) != AMOR_SUCCESS)
{
yCError(AMOR, "amor_get_actual_velocities() failed: %s", amor_error());
return false;
Expand All @@ -118,7 +118,7 @@ bool AmorControlboard::getEncoderSpeeds(double *spds)

AMOR_VECTOR7 velocities;

if (amor_get_actual_velocities(handle, &velocities) != AMOR_SUCCESS)
if (std::lock_guard<std::mutex> lock(handleMutex); amor_get_actual_velocities(handle, &velocities) != AMOR_SUCCESS)
{
yCError(AMOR, "amor_get_actual_velocities() failed: %s", amor_error());
return false;
Expand Down
Loading

0 comments on commit 1fb36fc

Please sign in to comment.