commit of the final code
This commit is contained in:
parent
df2a88a9cb
commit
612822571d
|
|
@ -0,0 +1,244 @@
|
|||
# Specify the minimum version of CMake required
|
||||
cmake_minimum_required(VERSION 3.0.2)
|
||||
# Define the project name
|
||||
project(poppy_ros)
|
||||
|
||||
# Option to compile as C++11 (commented out)
|
||||
## Compile as C++11, supported in ROS Kinetic and newer
|
||||
# add_compile_options(-std=c++11)
|
||||
|
||||
# Set the directory for OpenCV
|
||||
set (OpenCV_DIR "/usr/lib/opencv")
|
||||
|
||||
## Find catkin macros and libraries
|
||||
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
|
||||
## is used, also find other catkin packages
|
||||
# Find and include catkin components required for the project
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
std_msgs
|
||||
geometry_msgs
|
||||
message_generation
|
||||
sensor_msgs
|
||||
)
|
||||
|
||||
## System dependencies are found with CMake's conventions
|
||||
# Boost package can be included if needed (currently commented out)
|
||||
# find_package(Boost REQUIRED COMPONENTS system)
|
||||
# Find and include OpenCV components required for the project
|
||||
find_package(OpenCV REQUIRED COMPONENTS
|
||||
core
|
||||
)
|
||||
|
||||
|
||||
|
||||
# Instruction for including a setup.py if it exists
|
||||
## Uncomment this if the package has a setup.py. This macro ensures
|
||||
## modules and global scripts declared therein get installed
|
||||
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
|
||||
# catkin_python_setup()
|
||||
|
||||
################################################
|
||||
## Declare ROS messages, services and actions ##
|
||||
################################################
|
||||
|
||||
## To declare and build messages, services or actions from within this
|
||||
## package, follow these steps:
|
||||
## * Let MSG_DEP_SET be the set of packages whose message types you use in
|
||||
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
|
||||
## * In the file package.xml:
|
||||
## * add a build_depend tag for "message_generation"
|
||||
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
|
||||
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
|
||||
## but can be declared for certainty nonetheless:
|
||||
## * add a exec_depend tag for "message_runtime"
|
||||
## * In this file (CMakeLists.txt):
|
||||
## * add "message_generation" and every package in MSG_DEP_SET to
|
||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
||||
## * add "message_runtime" and every package in MSG_DEP_SET to
|
||||
## catkin_package(CATKIN_DEPENDS ...)
|
||||
## * uncomment the add_*_files sections below as needed
|
||||
## and list every .msg/.srv/.action file to be processed
|
||||
## * uncomment the generate_messages entry below
|
||||
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
|
||||
|
||||
## Generate messages in the 'msg' folder
|
||||
# add_message_files(
|
||||
# FILES
|
||||
# Message1.msg
|
||||
# Message2.msg
|
||||
# )
|
||||
|
||||
## Generate services in the 'srv' folder
|
||||
add_service_files(
|
||||
FILES
|
||||
ik.srv
|
||||
)
|
||||
|
||||
## Generate actions in the 'action' folder
|
||||
# add_action_files(
|
||||
# FILES
|
||||
# Action1.action
|
||||
# Action2.action
|
||||
# )
|
||||
|
||||
## Generate added messages and services with any dependencies listed here
|
||||
generate_messages(
|
||||
DEPENDENCIES
|
||||
geometry_msgs
|
||||
std_msgs
|
||||
|
||||
)
|
||||
|
||||
################################################
|
||||
## Declare ROS dynamic reconfigure parameters ##
|
||||
################################################
|
||||
|
||||
## To declare and build dynamic reconfigure parameters within this
|
||||
## package, follow these steps:
|
||||
## * In the file package.xml:
|
||||
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
|
||||
## * In this file (CMakeLists.txt):
|
||||
## * add "dynamic_reconfigure" to
|
||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
||||
## * uncomment the "generate_dynamic_reconfigure_options" section below
|
||||
## and list every .cfg file to be processed
|
||||
|
||||
## Generate dynamic reconfigure parameters in the 'cfg' folder
|
||||
# generate_dynamic_reconfigure_options(
|
||||
# cfg/DynReconf1.cfg
|
||||
# cfg/DynReconf2.cfg
|
||||
# )
|
||||
|
||||
###################################
|
||||
## catkin specific configuration ##
|
||||
###################################
|
||||
## The catkin_package macro generates cmake config files for your package
|
||||
## Declare things to be passed to dependent projects
|
||||
## INCLUDE_DIRS: uncomment this if your package contains header files
|
||||
## LIBRARIES: libraries you create in this project that dependent projects also need
|
||||
## CATKIN_DEPENDS: catkin_packages dependent projects also need
|
||||
## DEPENDS: system dependencies of this project that dependent projects also need
|
||||
catkin_package(
|
||||
INCLUDE_DIRS include
|
||||
LIBRARIES poppy_ros
|
||||
CATKIN_DEPENDS roscpp std_msgs message_runtime geometry_msgs
|
||||
# DEPENDS system_lib
|
||||
)
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
## Specify additional locations of header files
|
||||
## Your package locations should be listed before other locations------------------------------------------------
|
||||
include_directories(
|
||||
/home/maxence/ros/SOFTWARE/toolkit-dynamixel/include
|
||||
/home/maxence/ros/SOFTWARE/toolkit-kinematics/include
|
||||
${OpenCV_INCLUDE_DIRS}
|
||||
/home/maxence/ros/SOFTWARE/eigen/Eigen
|
||||
${catkin_INCLUDE_DIRS}
|
||||
/home/maxence/catkin_ws/src/poppy_ros/include
|
||||
)
|
||||
|
||||
## Declare a C++ library
|
||||
# add_library(${PROJECT_NAME}
|
||||
# src/${PROJECT_NAME}/poppy_ros.cpp
|
||||
# )
|
||||
|
||||
## Add cmake target dependencies of the library
|
||||
## as an example, code may need to be generated before libraries
|
||||
## either from message generation or dynamic reconfigure
|
||||
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
## Declare a C++ executable
|
||||
## With catkin_make all packages are built within a single CMake context
|
||||
## The recommended prefix ensures that target names across packages don't collide
|
||||
# add_executable(${PROJECT_NAME}_node src/poppy_ros_node.cpp)
|
||||
|
||||
add_executable(poppy_ros src/poppy_ros.cpp /home/maxence/ros/SOFTWARE/toolkit-dynamixel/src/DynamixelHandler.cpp)
|
||||
|
||||
add_executable(ik_server src/ik_server.cpp /home/maxence/ros/SOFTWARE/toolkit-kinematics/src/Kinematics.cpp /home/maxence/ros/SOFTWARE/toolkit-kinematics/src/RotationMatrix.cpp /home/maxence/ros/SOFTWARE/toolkit-kinematics/src/TranslationMatrix.cpp /home/maxence/ros/SOFTWARE/toolkit-kinematics/src/TransformationMatrix.cpp)
|
||||
|
||||
add_executable(ik_client src/ik_client.cpp)
|
||||
|
||||
|
||||
## Rename C++ executable without prefix
|
||||
## The above recommended prefix causes long target names, the following renames the
|
||||
## target back to the shorter version for ease of user use
|
||||
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
|
||||
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
|
||||
|
||||
## Add cmake target dependencies of the executable
|
||||
## same as for the library above
|
||||
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
## Specify libraries to link a library or executable target against
|
||||
# target_link_libraries(${PROJECT_NAME}_node
|
||||
# ${catkin_LIBRARIES}
|
||||
# )
|
||||
|
||||
|
||||
target_link_libraries(poppy_ros dxl_x64_cpp rt ${catkin_LIBRARIES})
|
||||
|
||||
target_link_libraries(ik_server ${catkin_LIBRARIES} ${OpenCV_LIBS})
|
||||
|
||||
target_link_libraries(ik_client ${catkin_LIBRARIES})
|
||||
|
||||
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
# all install targets should use catkin DESTINATION variables
|
||||
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
|
||||
|
||||
## Mark executable scripts (Python etc.) for installation
|
||||
## in contrast to setup.py, you can choose the destination
|
||||
# catkin_install_python(PROGRAMS
|
||||
# scripts/my_python_script
|
||||
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark executables for installation
|
||||
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
|
||||
# install(TARGETS ${PROJECT_NAME}_node
|
||||
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark libraries for installation
|
||||
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
|
||||
# install(TARGETS ${PROJECT_NAME}
|
||||
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark cpp header files for installation
|
||||
# install(DIRECTORY include/${PROJECT_NAME}/
|
||||
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
# FILES_MATCHING PATTERN "*.h"
|
||||
# PATTERN ".svn" EXCLUDE
|
||||
# )
|
||||
|
||||
## Mark other files for installation (e.g. launch and bag files, etc.)
|
||||
# install(FILES
|
||||
# # myfile1
|
||||
# # myfile2
|
||||
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
# )
|
||||
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
|
||||
## Add gtest based cpp test target and link libraries
|
||||
# catkin_add_gtest(${PROJECT_NAME}-test test/test_poppy_ros.cpp)
|
||||
# if(TARGET ${PROJECT_NAME}-test)
|
||||
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
|
||||
# endif()
|
||||
|
||||
## Add folders to be run by python nosetests
|
||||
# catkin_add_nosetests(test)
|
||||
|
||||
|
|
@ -0,0 +1,30 @@
|
|||
|
||||
#include <ros/ros.h>
|
||||
#include <geometry_msgs/Twist.h>
|
||||
#include <poppy_ros/ik.h>
|
||||
#include <math.h>
|
||||
#include <sensor_msgs/JointState.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
// Function to convert degrees to radians
|
||||
double deg2rad(double angle){
|
||||
return -angle / 180.0 * M_PI;
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
// Initialize ROS node
|
||||
ros::init(argc, argv, "ik_client");
|
||||
|
||||
// Check for correct number of arguments
|
||||
if (argc != 4) {
|
||||
ROS_INFO("Usage : rosrun poppy_ros ik_client x y z");
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Set up ROS node handle and service client
|
||||
ros::NodeHandle nh;
|
||||
ros::ServiceClient client = nh.serviceClient<poppy_ros::ik>("ik");
|
||||
|
||||
// Rest of the code goes here...
|
||||
}
|
||||
|
|
@ -0,0 +1,66 @@
|
|||
// Include necessary headers and libraries
|
||||
#include <ros/ros.h>
|
||||
// Include necessary headers and libraries
|
||||
#include <geometry_msgs/Twist.h>
|
||||
// Include necessary headers and libraries
|
||||
#include <poppy_ros/ik.h>
|
||||
// Include necessary headers and libraries
|
||||
#include <Kinematics.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
// Instantiate a Kinematics object for IK calculations
|
||||
Kinematics _kinematics;
|
||||
// Matrix to store the x, y, z position of the end effector
|
||||
cv::Mat _oXTarget(3, 1, CV_64F, 0.0); //Matrix storing the x, y, z position of the end effector
|
||||
|
||||
// Callback function for IK service
|
||||
bool ikCallback(poppy_ros::ik::Request &request, poppy_ros::ik::Response &response){
|
||||
|
||||
_oXTarget.at<double>(0,0) = request.x;
|
||||
_oXTarget.at<double>(1,0) = request.y;
|
||||
_oXTarget.at<double>(2,0) = request.z;
|
||||
|
||||
_kinematics.inverseKinematics(_oXTarget);
|
||||
vector<double> Qi = _kinematics.getQiValues();
|
||||
|
||||
response.j_val.linear.x = Qi[0];
|
||||
response.j_val.linear.y = Qi[1];
|
||||
response.j_val.linear.z = Qi[2];
|
||||
response.j_val.angular.x = Qi[3];
|
||||
response.j_val.angular.y = Qi[4];
|
||||
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
// Main function for the IK server
|
||||
int main(int argc, char** argv) {
|
||||
|
||||
// Check for the correct number of arguments
|
||||
if (argc != 3 && argc != 4){
|
||||
ROS_INFO("usage: rosrun poppy_ros ik_server <dhParamsFile> <ikParamsFile>");
|
||||
ROS_INFO("ex: rosrun poppy_ros ik_server /home/maxence/ros/SOFTWARE/poppy_dh.yaml /home/maxence/ros/SOFTWARE/poppy_ik.yaml");
|
||||
return 1;
|
||||
}
|
||||
if (argc >= 2){
|
||||
_kinematics.loadDHParameters(argv[1]);
|
||||
}
|
||||
if (argc >= 3){
|
||||
_kinematics.loadIKParameters(argv[2]);
|
||||
}
|
||||
cv::Mat currentEndEffPos(4, 1, CV_64F, 0.0f);
|
||||
currentEndEffPos = _kinematics.computeCurrentEndEffectorPosition();
|
||||
ROS_INFO("Current end-effector position = (%f, %f, %f)", currentEndEffPos.at<double>(0.0), currentEndEffPos.at<double>(1.0), currentEndEffPos.at<double>(2.0));
|
||||
|
||||
ros::init(argc,argv,"ik_server");
|
||||
ros::NodeHandle nh;
|
||||
ros::ServiceServer service = nh.advertiseService("ik",ikCallback);
|
||||
|
||||
ROS_INFO("IK Server launched");
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,69 @@
|
|||
// Include necessary headers and libraries
|
||||
#include <ros/ros.h>
|
||||
// Include necessary headers and libraries
|
||||
#include<geometry_msgs/Twist.h>
|
||||
// Include necessary headers and libraries
|
||||
#include<DynamixelHandler.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
// Instantiate a DynamixelHandler object for motor control
|
||||
DynamixelHandler dxlHandler; //0 degrees = pos 512
|
||||
|
||||
// Callback function to process command messages
|
||||
void cmdCallback(const geometry_msgs::Twist& cmd){
|
||||
//linear.x = j1,linear.y = j2, .. ,angular.z = j6
|
||||
vector<uint16_t> vec;
|
||||
vec.push_back((cmd.linear.x+150)*1024/300);
|
||||
vec.push_back((cmd.linear.y+150)*1024/300);
|
||||
vec.push_back((cmd.linear.z+150)*1024/300);
|
||||
vec.push_back((cmd.angular.x+150)*1024/300);
|
||||
vec.push_back((cmd.angular.y+150)*1024/300);
|
||||
vec.push_back((cmd.angular.z+150)*1024/300);
|
||||
|
||||
|
||||
|
||||
dxlHandler.sendTargetJointPosition(vec);
|
||||
}
|
||||
|
||||
// Main function for the ROS node
|
||||
int main(int argc, char** argv) {
|
||||
// Initialize the ROS node
|
||||
ros::init(argc,argv,"poppy_ros");
|
||||
ros::NodeHandle nh;
|
||||
ros::Subscriber cmd = nh.subscribe("joint_cmd",1,cmdCallback);
|
||||
ros::Publisher position = nh.advertise<geometry_msgs::Twist>("joint_position",1);
|
||||
|
||||
// Set device name and configuration for DynamixelHandler
|
||||
dxlHandler.setDeviceName("/dev/ttyUSB0");
|
||||
dxlHandler.setProtocolVersion(2.0);
|
||||
dxlHandler.openPort();
|
||||
dxlHandler.setBaudRate(1000000);
|
||||
dxlHandler.enableTorque(true);
|
||||
|
||||
ros::Rate loopRate(10);
|
||||
|
||||
while (ros::ok()) {
|
||||
geometry_msgs::Twist message;
|
||||
vector<uint16_t> curPos;
|
||||
dxlHandler.readCurrentJointPosition(curPos);
|
||||
|
||||
if (curPos.size()==6)
|
||||
{
|
||||
message.linear.x = curPos[0]*300/1024 - 150;
|
||||
message.linear.y = curPos[1]*300/1024 - 150;
|
||||
message.linear.z = curPos[2]*300/1024 - 150;
|
||||
message.angular.x = curPos[3]*300/1024 - 150;
|
||||
message.angular.y = curPos[4]*300/1024 - 150;
|
||||
message.angular.z = curPos[5]*300/1024 - 150;
|
||||
}
|
||||
|
||||
|
||||
position.publish(message);
|
||||
|
||||
ros::spinOnce();
|
||||
loopRate.sleep();
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,10 @@
|
|||
|
||||
# Request part of the service
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
|
||||
---
|
||||
|
||||
# Response part of the service
|
||||
geometry_msgs/Twist j_val
|
||||
Loading…
Reference in New Issue