Skip to content
Snippets Groups Projects
Commit 88866988 authored by David Tiede's avatar David Tiede
Browse files

Simple movement

parent d075ef64
No related branches found
No related tags found
No related merge requests found
......@@ -8,9 +8,9 @@ project(intro_package)
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
roscpp
std_msgs
moveit_ros_planning_interface
)
## System dependencies are found with CMake's conventions
......@@ -133,22 +133,20 @@ include_directories(
## 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}_intro_package src/intro_package.cpp)
add_executable(IntroPackage src/IntroPackage.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}_intro_package PROPERTIES OUTPUT_NAME intro_package PREFIX "")
# set_target_properties(${PROJECT_NAME}_intro_package PROPERTIES OUTPUT_NAME intro_package 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}_intro_package
${catkin_LIBRARIES}
)
target_link_libraries(IntroPackage ${catkin_LIBRARIES})
#############
## Install ##
......
<launch>
<include file="$(find panda_simulation)/launch/simulation.launch"/>
<include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
<node pkg="intro_package" type="IntroPackage" name="IntroPackageInstance" respawn="false" output="screen"/>
</launch>
\ No newline at end of file
<?xml version="1.0"?>
<package format="2">
<name>intro_package</name>
<version>0.0.0</version>
<version>0.0.1</version>
<description>The intro_package package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="tidenhub@todo.todo">tidenhub</maintainer>
<maintainer email="david.tiede@mailbox.tu-dresden.de">David Tiede</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
......@@ -49,16 +49,9 @@
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<depend>roscpp</depend>
<depend>std_msgs</depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
......
#include <moveit/move_group_interface/move_group_interface.h>
int main(int argc, char** argv)
{
ros::init(argc, argv, "intro_package");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
static const std::string PLANNING_GROUP = "panda_arm";
moveit::planning_interface::MoveGroupInterface group(PLANNING_GROUP);
ros::Duration(3.0).sleep();
group.setStartStateToCurrentState();
group.setPlanningTime(5.0);
std::vector<geometry_msgs::Pose> waypoints;
geometry_msgs::Pose start_pose = group.getCurrentPose().pose;
geometry_msgs::Pose target_pose_1 = group.getCurrentPose().pose;
target_pose_1.position.z = 0.6;
target_pose_1.position.y = -0.6;
target_pose_1.position.x = 0;
waypoints.push_back(target_pose_1);
moveit::planning_interface::MoveGroupInterface::Plan plan;
moveit_msgs::RobotTrajectory trajectory_msg;
group.computeCartesianPath(waypoints, 0.01, 0.0, trajectory_msg, true);
plan.trajectory_ = trajectory_msg;
group.execute(plan);
ROS_INFO("FINISHED");
ros::shutdown();
return 0;
}
#include <ros/ros.h>
int main(int argc, char** argv)
{
ros::init(argc, argv, "intro_package_node");
ros::NodeHandle node_handle("intro_package_ns");
ROS_INFO("HELLO WORLD");
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment