Skip to content

Commit

Permalink
Guard handle usage with shared mutex
Browse files Browse the repository at this point in the history 
roboticslab-uc3m/yarp-devices#181
  • Loading branch information
@PeterBowman
PeterBowman committed Feb 10, 2022
1 parent 9ee75b4 commit 00e915b
Show file tree
Hide file tree
Showing 4 changed files with 39 additions and 23 deletions.
6 changes: 4 additions & 2 deletions libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
#ifndef __AMOR_CARTESIAN_CONTROL_HPP__
#define __AMOR_CARTESIAN_CONTROL_HPP__

#include <mutex>
#include <vector>

#include <amor.h>
Expand Down Expand Up @@ -60,8 +61,9 @@ class AmorCartesianControl : public yarp::dev::DeviceDriver,
private:
bool checkJointVelocities(const std::vector<double> & qdot);

AMOR_HANDLE handle;
bool ownsHandle;
AMOR_HANDLE handle {AMOR_INVALID_HANDLE};
bool ownsHandle {true};
mutable std::mutex * handleMutex {nullptr};

yarp::dev::PolyDriver cartesianDevice;
ICartesianSolver * iCartesianSolver;
Expand Down
2 changes: 2 additions & 0 deletions libraries/YarpPlugins/AmorCartesianControl/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,8 @@ if(NOT SKIP_AmorCartesianControl)
ROBOTICSLAB::KinematicRepresentationLib
ROBOTICSLAB::KinematicsDynamicsInterfaces)

target_compile_features(AmorCartesianControl PRIVATE cxx_std_17)

yarp_install(TARGETS AmorCartesianControl
LIBRARY DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_DYNAMIC_PLUGINS_INSTALL_DIR}
ARCHIVE DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_STATIC_PLUGINS_INSTALL_DIR}
Expand Down
23 changes: 15 additions & 8 deletions libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,8 +54,9 @@ bool AmorCartesianControl::open(yarp::os::Searchable& config)
}

yarp::os::Value vHandle = config.find("handle");
yarp::os::Value vHandleMutex = config.find("handleMutex");

if (vHandle.isNull())
if (vHandle.isNull() || vHandleMutex.isNull())
{
yCInfo(AMOR) << "Creating own AMOR handle";

Expand All @@ -66,15 +67,17 @@ bool AmorCartesianControl::open(yarp::os::Searchable& config)

ownsHandle = true;
handle = amor_connect(const_cast<char *>(canLibrary.c_str()), canPort);
handleMutex = new std::mutex;
}
else
{
yCInfo(AMOR) << "Using external AMOR handle";
ownsHandle = false;
handle = *(reinterpret_cast<AMOR_HANDLE *>(const_cast<char *>(vHandle.asBlob())));
handle = *reinterpret_cast<AMOR_HANDLE *>(const_cast<char *>(vHandle.asBlob()));
handleMutex = reinterpret_cast<std::mutex *>(const_cast<char *>(vHandleMutex.asBlob()));
}

if (handle == AMOR_INVALID_HANDLE)
if (std::lock_guard<std::mutex> lock(*handleMutex); handle == AMOR_INVALID_HANDLE)
{
yCError(AMOR) << "Could not get AMOR handle:" << amor_error();
return false;
Expand All @@ -88,7 +91,7 @@ bool AmorCartesianControl::open(yarp::os::Searchable& config)
{
AMOR_JOINT_INFO jointInfo;

if (amor_get_joint_info(handle, i, &jointInfo) != AMOR_SUCCESS)
if (std::lock_guard<std::mutex> lock(*handleMutex); amor_get_joint_info(handle, i, &jointInfo) != AMOR_SUCCESS)
{
yCError(AMOR) << "amor_get_joint_info() failed:" << amor_error();
return false;
Expand Down Expand Up @@ -139,15 +142,19 @@ bool AmorCartesianControl::close()
{
if (handle != AMOR_INVALID_HANDLE)
{
std::unique_lock<std::mutex> lock(*handleMutex);
amor_emergency_stop(handle);
}

if (ownsHandle)
{
amor_release(handle);
if (ownsHandle)
{
amor_release(handle);
lock.unlock();
delete handleMutex;
}
}

handle = AMOR_INVALID_HANDLE;
handleMutex = nullptr;

return cartesianDevice.close();
}
Expand Down
31 changes: 18 additions & 13 deletions libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@ bool AmorCartesianControl::stat(std::vector<double> & x, int * state, double * t
{
AMOR_VECTOR7 positions;

if (amor_get_cartesian_position(handle, positions) != AMOR_SUCCESS)
if (std::lock_guard<std::mutex> lock(*handleMutex); amor_get_cartesian_position(handle, positions) != AMOR_SUCCESS)
{
yCError(AMOR) << "amor_get_cartesian_position() failed:" << amor_error();
return false;
Expand Down Expand Up @@ -54,7 +54,7 @@ bool AmorCartesianControl::inv(const std::vector<double> &xd, std::vector<double
{
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:" << amor_error();
return false;
Expand Down Expand Up @@ -95,7 +95,7 @@ bool AmorCartesianControl::movj(const std::vector<double> &xd)
positions[i] = KinRepresentation::degToRad(qd[i]);
}

if (amor_set_positions(handle, positions) != AMOR_SUCCESS)
if (std::lock_guard<std::mutex> lock(*handleMutex); amor_set_positions(handle, positions) != AMOR_SUCCESS)
{
yCError(AMOR) << "amor_set_positions() failed:" << amor_error();
return false;
Expand Down Expand Up @@ -141,7 +141,7 @@ bool AmorCartesianControl::movl(const std::vector<double> &xd)
{
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:" << amor_error();
return false;
Expand Down Expand Up @@ -187,7 +187,7 @@ bool AmorCartesianControl::movl(const std::vector<double> &xd)
positions[4] = xd_rpy[4];
positions[5] = xd_rpy[5];

if (amor_set_cartesian_positions(handle, positions) != AMOR_SUCCESS)
if (std::lock_guard<std::mutex> lock(*handleMutex); amor_set_cartesian_positions(handle, positions) != AMOR_SUCCESS)
{
yCError(AMOR) << "amor_set_cartesian_positions() failed:" << amor_error();
return false;
Expand Down Expand Up @@ -231,7 +231,7 @@ bool AmorCartesianControl::movv(const std::vector<double> &xdotd)
velocities[4] = -xdotd_rpy[5];
velocities[5] = xdotd_rpy[3];

if (amor_set_cartesian_velocities(handle, velocities) != AMOR_SUCCESS)
if (std::lock_guard<std::mutex> lock(*handleMutex); amor_set_cartesian_velocities(handle, velocities) != AMOR_SUCCESS)
{
yCError(AMOR) << "amor_set_cartesian_velocities() failed:" << amor_error();
return false;
Expand Down Expand Up @@ -264,7 +264,7 @@ bool AmorCartesianControl::stopControl()
{
currentState = VOCAB_CC_NOT_CONTROLLING;

if (amor_controlled_stop(handle) != AMOR_SUCCESS)
if (std::lock_guard<std::mutex> lock(*handleMutex); amor_controlled_stop(handle) != AMOR_SUCCESS)
{
yCError(AMOR) << "amor_controlled_stop() failed:" << amor_error();
return false;
Expand Down Expand Up @@ -296,7 +296,10 @@ bool AmorCartesianControl::wait(double timeout)
break;
}

res = amor_get_movement_status(handle, &status);
{
std::lock_guard<std::mutex> lock(*handleMutex);
res = amor_get_movement_status(handle, &status);
}

if (res == AMOR_FAILED)
{
Expand Down Expand Up @@ -343,7 +346,7 @@ bool AmorCartesianControl::act(int command)
return false;
}

if (amor_command(handle) != AMOR_SUCCESS)
if (std::lock_guard<std::mutex> lock(*handleMutex); amor_command(handle) != AMOR_SUCCESS)
{
yCError(AMOR) << "amor_command() failed:" << amor_error();
return false;
Expand All @@ -358,7 +361,7 @@ void AmorCartesianControl::twist(const std::vector<double> &xdot)
{
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:" << amor_error();
return;
Expand All @@ -379,6 +382,7 @@ void AmorCartesianControl::twist(const std::vector<double> &xdot)

if (!checkJointVelocities(qdot))
{
std::lock_guard<std::mutex> lock(*handleMutex);
amor_controlled_stop(handle);
return;
}
Expand All @@ -390,7 +394,7 @@ void AmorCartesianControl::twist(const std::vector<double> &xdot)
velocities[i] = KinRepresentation::degToRad(qdot[i]);
}

if (amor_set_velocities(handle, velocities) != AMOR_SUCCESS)
if (std::lock_guard<std::mutex> lock(*handleMutex); amor_set_velocities(handle, velocities) != AMOR_SUCCESS)
{
yCError(AMOR) << "amor_set_velocities() failed:" << amor_error();
return;
Expand All @@ -403,7 +407,7 @@ void AmorCartesianControl::pose(const std::vector<double> &x, double interval)
{
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:" << amor_error();
return;
Expand Down Expand Up @@ -465,6 +469,7 @@ void AmorCartesianControl::pose(const std::vector<double> &x, double interval)

if (!checkJointVelocities(qdot))
{
std::lock_guard<std::mutex> lock(*handleMutex);
amor_controlled_stop(handle);
return;
}
Expand All @@ -476,7 +481,7 @@ void AmorCartesianControl::pose(const std::vector<double> &x, double interval)
velocities[i] = KinRepresentation::degToRad(qdot[i]);
}

if (amor_set_velocities(handle, velocities) != AMOR_SUCCESS)
if (std::lock_guard<std::mutex> lock(*handleMutex); amor_set_velocities(handle, velocities) != AMOR_SUCCESS)
{
yCError(AMOR) << "amor_set_velocities() failed:" << amor_error();
return;
Expand Down

0 comments on commit 00e915b

Please sign in to comment.