Skip to content

Commit

Permalink
Removed all references to ROS1
Browse files Browse the repository at this point in the history 
Now YARP 3.11.0 (with yarp::dev::ReturnValue) is required
  • Loading branch information
@randaz81
randaz81 committed Mar 3, 2025
1 parent 2403041 commit de7a888
Show file tree
Hide file tree
Showing 11 changed files with 16 additions and 252 deletions.
3 changes: 1 addition & 2 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,7 @@ set_property(GLOBAL PROPERTY USE_FOLDERS ON)
list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake")

find_package(YCM)
find_package(YARP 3.7 REQUIRED COMPONENTS os sig dev math idl_tools)
find_package(YARP_rosmsg)
find_package(YARP 3.11.0 REQUIRED COMPONENTS os sig dev math idl_tools)

find_package(ICUB REQUIRED)
list(APPEND CMAKE_MODULE_PATH ${ICUB_MODULE_PATH})
Expand Down
11 changes: 6 additions & 5 deletions cermod/cerOdometry/CerOdometry.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
#include <yarp/math/Rand.h>
#include <iostream>

using namespace yarp::dev;
using namespace cer::dev;

namespace {
Expand Down Expand Up @@ -49,7 +50,7 @@ bool CerOdometry::close()
return true;
}

bool CerOdometry::getOdometry(yarp::dev::OdometryData& odom, double* timestamp)
ReturnValue CerOdometry::getOdometry(yarp::dev::OdometryData& odom, double* timestamp)
{
std::lock_guard lock(m_odometry_mutex);
odom.odom_x = m_odometryData.odom_x;
Expand All @@ -65,10 +66,10 @@ bool CerOdometry::getOdometry(yarp::dev::OdometryData& odom, double* timestamp)
{
*timestamp = m_time_encoders;
}
return true;
return ReturnValue_ok;
}

bool CerOdometry::resetOdometry()
ReturnValue CerOdometry::resetOdometry()
{
if (ienct) {
ienct->getEncoder(0, &encL_offset);
Expand All @@ -80,7 +81,7 @@ bool CerOdometry::resetOdometry()
encvel_estimator->reset();
}
yCInfo(CERODOM,"Odometry reset done");
return true;
return ReturnValue_ok;
}

bool CerOdometry::open(yarp::os::Searchable& config)
Expand All @@ -91,7 +92,7 @@ bool CerOdometry::open(yarp::os::Searchable& config)
{
yCError(CERODOM) << "yarp network not found";
std::cerr << "Sorry YARP network does not seem to be available, is the yarp server available?\n";
return -1;
return false;
}

if (!config.check("period"))
Expand Down
5 changes: 3 additions & 2 deletions cermod/cerOdometry/CerOdometry.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
#include <yarp/dev/PolyDriver.h>
#include <yarp/dev/IEncodersTimed.h>
#include <yarp/dev/WrapperSingle.h>
#include <yarp/dev/ReturnValue.h>

constexpr double default_period = 0.02;
#ifndef RAD2DEG
Expand Down Expand Up @@ -102,8 +103,8 @@ class cer::dev::CerOdometry :
bool close() override;

// IOdometry2D
bool getOdometry(yarp::dev::OdometryData& odom, double* timestamp = nullptr) override;
bool resetOdometry() override;
yarp::dev::ReturnValue getOdometry(yarp::dev::OdometryData& odom, double* timestamp = nullptr) override;
yarp::dev::ReturnValue resetOdometry() override;

// auxiliar

Expand Down
8 changes: 1 addition & 7 deletions modules/robotJoystickControl/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,12 +18,6 @@ include_directories(${GSL_INCLUDE_DIRS})

add_executable(${PROJECTNAME} ${folder_source} ${folder_header})

if(YARP_rosmsg_FOUND)
add_definitions(-DROS_MSG=1)
target_link_libraries(${PROJECTNAME} ctrlLib ${GSL_LIBRARIES} YARP::YARP_rosmsg ${YARP_LIBRARIES})
else()
target_link_libraries(${PROJECTNAME} ctrlLib ${GSL_LIBRARIES} ${YARP_LIBRARIES})
endif()

target_link_libraries(${PROJECTNAME} ctrlLib ${GSL_LIBRARIES} ${YARP_LIBRARIES})
install(TARGETS ${PROJECTNAME} DESTINATION bin)

118 changes: 1 addition & 117 deletions modules/robotJoystickControl/controlThread.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,102 +27,6 @@
using namespace yarp::sig;
using namespace yarp::math;

#ifdef ROS_MSG
using namespace yarp::rosmsg;

void ControlThread::updateRVIZ(const Vector &xd, const Vector &od)
{
double yarpTimeStamp = yarp::os::Time::now();
uint64_t time;
uint64_t nsec_part;
uint64_t sec_part;
TickTime ret;
time = (uint64_t)(yarpTimeStamp * 1000000000UL);
nsec_part = (time % 1000000000UL);
sec_part = (time / 1000000000UL);
if (sec_part > std::numeric_limits<unsigned int>::max())
{
yWarning() << "Timestamp exceeded the 64 bit representation, resetting it to 0";
sec_part = 0;
}

visualization_msgs::MarkerArray& markerarray = rosPublisherPort.prepare();
markerarray.markers.clear();
visualization_msgs::Marker marker;
marker.header.frame_id = "mobile_base_body_link";
marker.header.stamp.sec = (yarp::os::NetUint32) sec_part;
marker.header.stamp.nsec = (yarp::os::NetUint32) nsec_part;
marker.ns = "cer-teleop_namespace";
marker.type = visualization_msgs::Marker::SPHERE;
marker.action = visualization_msgs::Marker::ADD;

//center
Quaternion q;
q.fromRotationMatrix(axis2dcm(od));
marker.id = 1;
marker.pose.position.x = xd[0];
marker.pose.position.y = xd[1];
marker.pose.position.z = xd[2];
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0;
marker.pose.orientation.w = 1.0;
marker.scale.x = 0.05;
marker.scale.y = 0.05;
marker.scale.z = 0.05;
marker.color.a = 0.5;
marker.color.r = 0.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
markerarray.markers.push_back(marker);

/*
//x
marker.id = 2;
marker.pose.orientation.x = q[3];
marker.pose.orientation.y = q[0];
marker.pose.orientation.z = q[1];
marker.pose.orientation.w = q[2];
marker.scale.x = 0.5;
marker.scale.y = 0.05;
marker.scale.z = 0.05;
marker.color.r = 1.0;
marker.color.g = 0.0;
marker.color.b = 0.0;
markerarray.markers.push_back(marker);
//y
marker.id = 3;
marker.pose.orientation.x = q[3];
marker.pose.orientation.y = q[0];
marker.pose.orientation.z = q[1];
marker.pose.orientation.w = q[2];
marker.scale.x = 0.05;
marker.scale.y = 0.5;
marker.scale.z = 0.05;
marker.color.r = 0.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
markerarray.markers.push_back(marker);
//z
marker.id = 4;
marker.pose.orientation.x = q[3];
marker.pose.orientation.y = q[0];
marker.pose.orientation.z = q[1];
marker.pose.orientation.w = q[2];
marker.scale.x = 0.05;
marker.scale.y = 0.05;
marker.scale.z = 0.5;
marker.color.r = 0.0;
marker.color.g = 0.0;
marker.color.b = 1.0;
markerarray.markers.push_back(marker);
*/
rosPublisherPort.write();
}
#endif

void ControlThread::reachingHandler(string arm_type, const bool b, const Vector &pos, const Vector &rpy)
{
///yDebug() << "reaching handler";
Expand Down Expand Up @@ -151,10 +55,6 @@ void ControlThread::reachingHandler(string arm_type, const bool b, const Vector
goToPose(arm_type, xd, od);
//updateGazebo(xd, od);

#ifdef ROS_MSG
updateRVIZ(xd, od);
#endif

/* if (c0 != 0)
{
stopReaching();
Expand Down Expand Up @@ -564,15 +464,7 @@ bool ControlThread::threadInit()
{
error_status = false;
bool autoconnect = false;
#ifdef ROS_MSG
rosNode = new yarp::os::Node(localName);
if (!rosPublisherPort.topic(localName+"_marker"))
{
yError()<<"Unable to publish data on " << localName << "_marker" << " topic";
yWarning("Check your yarp-ROS network configuration");
return false;
}
#endif

//open the joystick port
port_joystick_control.open(localName + "/joystick:i");

Expand Down Expand Up @@ -929,14 +821,6 @@ void ControlThread::goToPose(string arm_type, const Vector &xd, const Vector &od

void ControlThread::threadRelease()
{
#ifdef ROS_MSG
if (rosNode)
{
delete rosNode;
rosNode = 0;
}
#endif

if (interface_torso_tripod_iCmd)
{
interface_torso_tripod_iCmd->setControlMode(0, VOCAB_CM_POSITION);
Expand Down
13 changes: 0 additions & 13 deletions modules/robotJoystickControl/controlThread.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,12 +31,6 @@ using namespace std;
using namespace yarp::os;
using namespace yarp::dev;

#ifdef ROS_MSG
#include <yarp/rosmsg/visualization_msgs/Marker.h>
#include <yarp/rosmsg/visualization_msgs/MarkerArray.h>
using namespace yarp::rosmsg;
#endif

#define MAX_LINEAR_VEL 0.4 // maximum linear velocity (m/s)
#define MAX_ANGULAR_VEL 24.0 // maximum angular velocity (deg/s)

Expand Down Expand Up @@ -67,10 +61,6 @@ class ControlThread : public yarp::os::PeriodicThread
PolyDriver *driver_torso_equiv;
PolyDriver *driver_left_hand;
PolyDriver *driver_right_hand;
#ifdef ROS_MSG
Publisher<visualization_msgs::MarkerArray> rosPublisherPort;
Node *rosNode;
#endif

BufferedPort<Bottle> port_joystick_control;

Expand Down Expand Up @@ -142,9 +132,6 @@ class ControlThread : public yarp::os::PeriodicThread
void velMoveHandler(const bool b, std::vector<int> joints, double speed, IControlMode * imod, IVelocityControl* ivel);
void reachingHandler(string arm_type, const bool b, const yarp::sig::Vector &pos, const yarp::sig::Vector &rpy);
void saturate(double& v, double sat_lim);
#ifdef ROS_MSG
void updateRVIZ(const yarp::sig::Vector &xd, const yarp::sig::Vector &od);
#endif
void getCartesianArmPositions(bool blocking);

void option1(double* axis);
Expand Down
2 changes: 1 addition & 1 deletion modules/robotJoystickControl/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -211,7 +211,7 @@ class CtrlModule: public RFModule
robot_status robstatus;
static int life_counter = 0;
char text[1000];
int off = sprintf(text, "* Life: %4d", life_counter);
int off = snprintf(text, 1000, "* Life: %4d", life_counter);
if (control_thr)
{
control_thr->printStats();
Expand Down
10 changes: 3 additions & 7 deletions modules/teleop/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,14 +6,10 @@ project(cer_teleop)
set(CMAKE_CXX_STANDARD 11)


include_directories(${PROJECT_SOURCE_DIR}/ros_messages ${PROJECT_SOURCE_DIR})
include_directories(${PROJECT_SOURCE_DIR})
add_executable(${PROJECT_NAME} main.cpp handThread.cpp ParamParser.h)
target_compile_definitions(${PROJECT_NAME} PRIVATE _USE_MATH_DEFINES)
if(YARP_rosmsg_FOUND)
add_definitions(-DROS_MSG=1)
target_link_libraries(${PROJECT_NAME} YARP::YARP_rosmsg ${YARP_LIBRARIES})
else()
target_link_libraries(${PROJECT_NAME} ${YARP_LIBRARIES})
endif()

target_link_libraries(${PROJECT_NAME} ${YARP_LIBRARIES})
install(TARGETS ${PROJECT_NAME} DESTINATION bin)

Loading

0 comments on commit de7a888

Please sign in to comment.