Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[WIP] Implemented moveit based distance computation #104

Closed
wants to merge 3 commits into from
Closed

[WIP] Implemented moveit based distance computation #104

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
f3162f1
Implemented moveit based distance computation
MaChristoph Apr 12, 2016
61e56ae
Work in progress: verify if the nearest_point_frame_vector is equal t…
MaChristoph Apr 19, 2016
992054c
First implementation of CA with base_active
MaChristoph May 10, 2016
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
18 changes: 13 additions & 5 deletions cob_obstacle_distance/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,30 +21,38 @@ catkin_package(
CATKIN_DEPENDS cob_control_msgs cob_srvs dynamic_reconfigure eigen_conversions geometry_msgs kdl_conversions kdl_parser moveit_msgs roscpp roslib sensor_msgs shape_msgs std_msgs tf_conversions tf urdf visualization_msgs
DEPENDS assimp Boost fcl
INCLUDE_DIRS include
LIBRARIES parsers marker_shapes_management
LIBRARIES parsers marker_shapes_management distance_manager
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

add the helper_functions library here, too

)

### BUILD ###
include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${EIGEN_INCLUDE_DIRS} ${FCL_INCLUDE_DIRS} ${orocos_kdl_INCLUDE_DIRS} ${ASSIMP_INCLUDE_DIRS})

add_library(helper_functions src/helpers/helper_functions.cpp)
add_dependencies(helper_functions ${catkin_EXPORTED_TARGETS})
target_link_libraries(helper_functions ${catkin_LIBRARIES})

add_library(parsers src/parsers/stl_parser.cpp src/parsers/mesh_parser.cpp)
add_dependencies(parsers ${catkin_EXPORTED_TARGETS})
target_link_libraries(parsers assimp ${fcl_LIBRARIES} ${catkin_LIBRARIES})
target_link_libraries(parsers helper_functions assimp ${fcl_LIBRARIES} ${catkin_LIBRARIES})

add_library(marker_shapes_management src/marker_shapes/marker_shapes_impl.cpp src/marker_shapes/marker_shapes_interface.cpp src/shapes_manager.cpp src/link_to_collision.cpp)
add_dependencies(marker_shapes_management ${catkin_EXPORTED_TARGETS})
target_link_libraries(marker_shapes_management parsers ${fcl_LIBRARIES} ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES})

add_executable(cob_obstacle_distance src/cob_obstacle_distance.cpp src/helpers/helper_functions.cpp src/distance_manager.cpp src/chainfk_solvers/advanced_chainfksolver_recursive.cpp)
add_library(distance_manager src/distance_manager.cpp src/chainfk_solvers/advanced_chainfksolver_recursive.cpp)
add_dependencies(distance_manager ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(distance_manager marker_shapes_management ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES})

add_executable(cob_obstacle_distance src/cob_obstacle_distance.cpp)
add_dependencies(cob_obstacle_distance ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(cob_obstacle_distance parsers marker_shapes_management ${fcl_LIBRARIES} ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES})
target_link_libraries(cob_obstacle_distance helper_functions parsers marker_shapes_management distance_manager ${fcl_LIBRARIES} ${catkin_LIBRARIES})

add_executable(debug_obstacle_distance_node src/debug/debug_obstacle_distance_node.cpp)
add_dependencies(debug_obstacle_distance_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(debug_obstacle_distance_node ${catkin_LIBRARIES})

### Install ###
install(TARGETS cob_obstacle_distance parsers marker_shapes_management debug_obstacle_distance_node
install(TARGETS cob_obstacle_distance parsers marker_shapes_management distance_manager debug_obstacle_distance_node
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

add the helper_functions library here, too

ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
Expand Down
1 change: 1 addition & 0 deletions cob_obstacle_distance/src/debug/debug_obstacle_distance_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -93,6 +93,7 @@ std::string chain_base_link_;
marker_vector.id = 42;
marker_vector.header.frame_id = chain_base_link_;


Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

remove 😉

marker_vector.scale.x = 0.01;
marker_vector.scale.y = 0.05;

Expand Down
2 changes: 1 addition & 1 deletion cob_obstacle_distance/src/distance_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -90,7 +90,7 @@ int DistanceManager::init()
return -1;
}

if(!nh_.getParam("joint_names", this->joints_))
if(!nh_.getParam("/arm_left/joint_names", this->joints_))
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

do not use hardcoded namespaces....
It's ok for testing but in the end should be kept as general as possible...if you need joint_states within this library, you could also subscribe to /joint_states topic which has all joint_states included....you just would need a proper parsing/mapping strategy to extract the joint_states relevant for this library

{
ROS_ERROR("Failed to get parameter \"joint_names\".");
return -2;
Expand Down
6 changes: 3 additions & 3 deletions cob_twist_controller/src/cob_twist_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -142,7 +142,7 @@ bool CobTwistController::initialize()
{
twist_controller_params_.frame_names.push_back(chain_.getSegment(i).getName());
}
register_link_client_ = nh_.serviceClient<cob_srvs::SetString>("obstacle_distance/registerLinkOfInterest");
register_link_client_ = nh_.serviceClient<cob_srvs::SetString>("/register_links");
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

is ok, as the moveit-based node is a single global instance

register_link_client_.waitForExistence(ros::Duration(5.0));
twist_controller_params_.constraint_ca = CA_OFF;

Expand All @@ -164,7 +164,7 @@ bool CobTwistController::initialize()
ros::Duration(1.0).sleep();

/// initialize ROS interfaces
obstacle_distance_sub_ = nh_.subscribe("obstacle_distance", 1, &CallbackDataMediator::distancesToObstaclesCallback, &callback_data_mediator_);
obstacle_distance_sub_ = nh_.subscribe("/obstacle_distances", 1, &CallbackDataMediator::distancesToObstaclesCallback, &callback_data_mediator_);
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

is ok, as the moveit-based node is a single global instance

jointstate_sub_ = nh_.subscribe("joint_states", 1, &CobTwistController::jointstateCallback, this);
twist_sub_ = nh_twist.subscribe("command_twist", 1, &CobTwistController::twistCallback, this);
twist_stamped_sub_ = nh_twist.subscribe("command_twist_stamped", 1, &CobTwistController::twistStampedCallback, this);
Expand Down Expand Up @@ -309,7 +309,7 @@ void CobTwistController::checkSolverAndConstraints(cob_twist_controller::TwistCo
{
if (!register_link_client_.exists())
{
ROS_ERROR("ServiceServer 'obstacle_distance/registerLinkOfInterest' does not exist. CA not possible");
ROS_ERROR("ServiceServer '/register_links' does not exist. CA not possible");
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

is ok, as the moveit-based node is a single global instance

twist_controller_params_.constraint_ca = CA_OFF;
config.constraint_ca = static_cast<int>(twist_controller_params_.constraint_ca);
warning = true;
Expand Down