Skip to content
Snippets Groups Projects
Commit 731338a6 authored by Sebastian Ebert's avatar Sebastian Ebert
Browse files

added node for constraint path planning (box-based)

parent e186f737
No related branches found
No related tags found
No related merge requests found
......@@ -59,15 +59,24 @@ include_directories(${catkin_INCLUDE_DIRS})
add_executable(robot_control_node src/robot_control_node.cpp)
add_executable(box_publisher_node src/box_publisher.cpp)
add_executable(robot_state_initializer_node src/robot_state_initializer.cpp)
add_executable(move_group_interface_tutorial src/move_group_interface_tutorial.cpp)
#add_executable(move_group_interface_tutorial src/move_group_interface_tutorial.cpp)
#add_executable(motion_planning_api_tutorial src/motion_planning_api_tutorial.cpp)
add_executable(constraint_planner src/constraint_planner.cpp)
target_link_libraries(robot_control_node ${catkin_LIBRARIES} boost_filesystem ${JSONCPP_LIBRARIES})
target_link_libraries(box_publisher_node ${catkin_LIBRARIES})
target_link_libraries(robot_state_initializer_node ${catkin_LIBRARIES})
target_link_libraries(move_group_interface_tutorial ${catkin_LIBRARIES} ${Boost_LIBRARIES})
#target_link_libraries(move_group_interface_tutorial ${catkin_LIBRARIES} ${Boost_LIBRARIES})
#target_link_libraries(motion_planning_api_tutorial ${catkin_LIBRARIES} ${Boost_LIBRARIES})
target_link_libraries(constraint_planner ${catkin_LIBRARIES} ${Boost_LIBRARIES})
# add custom controller as library
add_library(${PROJECT_NAME}_controllers_lib src/joint_position_controller.cpp)
# Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}_controllers_lib ${catkin_LIBRARIES})
#install(TARGETS move_group_interface_tutorial DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
#install(TARGETS motion_planning_api_tutorial DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
#install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
\ No newline at end of file
......@@ -27,7 +27,6 @@
<!-- Load joint controller configurations from YAML file to parameter server -->
<rosparam file="$(find panda_simulation)/config/panda_control.yaml" command="load" />
<!-- load the controllers -->
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller panda_arm_controller" />
<node if="$(arg load_gripper)" name="controller_spawner_hand" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="panda_hand_controller" />
......@@ -58,4 +57,14 @@
<!-- run custom node for automatic intialization -->
<node pkg="panda_simulation" type="robot_state_initializer_node" name="robot_state_initializer_node" />
<!--<node name="move_group_interface_tutorial" pkg="panda_simulation" type="move_group_interface_tutorial" respawn="false" output="screen"/>-->
<node name="constraint_planner" pkg="panda_simulation" type="constraint_planner" respawn="false" output="screen"/>
<!--<node name="motion_planning_api_tutorial" pkg="panda_simulation" type="motion_planning_api_tutorial" respawn="false" output="screen">
<rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
<param name="/planning_plugin" value="ompl_interface/OMPLPlanner"/>
<rosparam command="load" file="$(find panda_moveit_config)/config/ompl_planning.yaml"/>
</node>-->
</launch>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
int main(int argc, char** argv)
{
ros::init(argc, argv, "CONSTRAINT PLANNER");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
static const std::string PLANNING_GROUP = "panda_arm";
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
const robot_state::JointModelGroup* joint_model_group =
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
// Visualization Setup
namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
visual_tools.deleteAllMarkers();
visual_tools.loadRemoteControl();
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.75;
visual_tools.publishText(text_pose, "Sample constraint planner node", rvt::WHITE, rvt::XLARGE);
// Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
visual_tools.trigger();
// Getting Basic Information
ROS_INFO_NAMED("constraint_planner", "Planning frame: %s", move_group.getPlanningFrame().c_str());
ROS_INFO_NAMED("constraint_planner", "End effector link: %s", move_group.getEndEffectorLink().c_str());
ROS_INFO_NAMED("constraint_planner", "Available Planning Groups:");
std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
std::ostream_iterator<std::string>(std::cout, ", "));
// Define a collision object ROS message.
moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = move_group.getPlanningFrame();
// The id of the object is used to identify it.
collision_object.id = "box1";
// Define a box to add to the world.
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.4;
primitive.dimensions[1] = 0.1;
primitive.dimensions[2] = 0.4;
// Define a pose for the box (specified relative to frame_id)
geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x = 0.4;
box_pose.position.y = -0.2;
box_pose.position.z = 1.0;
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(collision_object);
// Now, let's add the collision object into the world
ROS_INFO_NAMED("tutorial", "Add an object into the world");
planning_scene_interface.addCollisionObjects(collision_objects);
// Show text in RViz of status
visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");
// Now when we plan a trajectory it will avoid the obstacle
move_group.setStartState(*move_group.getCurrentState());
geometry_msgs::Pose another_pose;
another_pose.orientation.w = 1.0;
another_pose.position.x = 0.4;
another_pose.position.y = -0.4;
another_pose.position.z = 0.9;
move_group.setPoseTarget(another_pose);
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("constraint_planner", "Visualizing constraint plan %s", success ? "" : "FAILED");
visual_tools.prompt("Press 'next' to move the simulated robot.");
visual_tools.trigger();
// Move the simulated robot in gazebo
move_group.move();
ros::shutdown();
return 0;
}
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment