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

refactoring

parent 58c81c7e
No related branches found
No related tags found
No related merge requests found
Showing
with 4381 additions and 304 deletions
...@@ -71,25 +71,28 @@ include_directories(${catkin_INCLUDE_DIRS}) ...@@ -71,25 +71,28 @@ include_directories(${catkin_INCLUDE_DIRS})
# add custom controller as library # add custom controller as library
add_library(${PROJECT_NAME}_controllers_lib src/joint_position_controller.cpp) add_library(${PROJECT_NAME}_controllers_lib src/joint_position_controller.cpp)
add_library(linkstate src/linkstate.pb.cc src/linkstate.pb.h) add_library(linkstate src/messages/linkstate.pb.cc src/messages/linkstate.pb.h)
add_library(dataconfig src/messages/dataconfig.pb.cc src/messages/dataconfig.pb.h)
add_library(robotconfig src/messages/robotconfig.pb.cc src/messages/robotconfig.pb.h)
# Specify libraries to link a library or executable target against # Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}_controllers_lib ${catkin_LIBRARIES}) target_link_libraries(${PROJECT_NAME}_controllers_lib ${catkin_LIBRARIES})
add_executable(robot_control_node src/robot_control_node.cpp) 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(robot_state_initializer_node src/robot_state_initializer.cpp)
add_executable(SafetyAwarePlanner src/SafetyAwarePlanner.cpp src/SafetyEnvironmentCreator.cpp) add_executable(SafetyAwarePlanner src/SafetyAwarePlanner.cpp src/SafetyEnvironmentCreator.cpp)
add_executable(SampleConstraintPlanner src/SampleConstraintPlanner.cpp) add_executable(SampleConstraintPlanner src/samples/SampleConstraintPlanner.cpp)
add_executable(SampleTimedCartesianPlanner src/samples/SampleTimedCartesianPlanner.cpp)
add_executable(RobotStateProvider src/RobotStateProvider.cpp src/mem_persistence.cpp) add_executable(RobotStateProvider src/RobotStateProvider.cpp src/mem_persistence.cpp)
add_executable(TimedCartesianPlanner src/TimedCartesianPlanner.cpp) add_executable(TimedCartesianPlanner src/TimedCartesianPlanner.cpp)
add_executable(MqttToRosNode src/MqttToRosNode.cpp) add_executable(MqttToRosNode src/MqttToRosNode.cpp)
add_executable(MqttRosConnectionTestNode src/MqttRosConnectionTestNode.cpp)
target_link_libraries(robot_control_node ${catkin_LIBRARIES} boost_filesystem ${JSONCPP_LIBRARIES}) 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(robot_state_initializer_node ${catkin_LIBRARIES})
target_link_libraries(SafetyAwarePlanner ${catkin_LIBRARIES}) target_link_libraries(SafetyAwarePlanner ${catkin_LIBRARIES})
target_link_libraries(SampleConstraintPlanner ${catkin_LIBRARIES} ${Boost_LIBRARIES}) target_link_libraries(SampleConstraintPlanner ${catkin_LIBRARIES})
target_link_libraries(SampleTimedCartesianPlanner ${catkin_LIBRARIES})
target_link_libraries(RobotStateProvider LINK_PUBLIC target_link_libraries(RobotStateProvider LINK_PUBLIC
${catkin_LIBRARIES} ${catkin_LIBRARIES}
${PahoMqtt3c} ${PahoMqtt3c}
...@@ -105,5 +108,17 @@ target_link_libraries(MqttToRosNode LINK_PUBLIC ...@@ -105,5 +108,17 @@ target_link_libraries(MqttToRosNode LINK_PUBLIC
${PahoMqtt3cs} ${PahoMqtt3cs}
${PahoMqtt3a} ${PahoMqtt3a}
${PahoMqtt3as} ${PahoMqtt3as}
${PahoMqttCpp}) ${PahoMqttCpp}
${Protobuf_LIBRARIES}
dataconfig
robotconfig)
target_link_libraries(MqttRosConnectionTestNode LINK_PUBLIC
${catkin_LIBRARIES}
${PahoMqtt3c}
${PahoMqtt3cs}
${PahoMqtt3a}
${PahoMqtt3as}
${PahoMqttCpp}
${Protobuf_LIBRARIES}
dataconfig)
target_link_libraries(TimedCartesianPlanner ${catkin_LIBRARIES}) target_link_libraries(TimedCartesianPlanner ${catkin_LIBRARIES})
\ No newline at end of file
<launch>
<param name="robot_description" command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm_hand.urdf.xacro" />
<!-- GAZEBO arguments -->
<arg name="paused" default="false" />
<arg name="use_sim_time" default="true" />
<arg name="gui" default="true" />
<arg name="headless" default="false" />
<arg name="debug" default="false" />
<arg name="load_gripper" default="true" />
<!--launch GAZEBO with own world configuration -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<!-- <arg name="world_name" value="$(find robotics_assisted_tomography)/worlds/tomography.world"/> -->
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)" />
<arg name="use_sim_time" value="$(arg use_sim_time)" />
<arg name="headless" value="$(arg headless)" />
</include>
<node name="rqt_console" pkg="rqt_console" type="rqt_console" />
<!-- <node name="rqt_logger_level" pkg="rqt_logger_level" type="rqt_logger_level" /> -->
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model panda" />
<!-- 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" />
<!-- convert joint states to TF transforms for rviz, etc -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
<include file="$(find panda_moveit_config)/launch/planning_context.launch">
<arg name="load_robot_description" value="true" />
<arg name="load_gripper" value="$(arg load_gripper)" />
</include>
<include file="$(find panda_moveit_config)/launch/move_group.launch">
<arg name="load_gripper" value="$(arg load_gripper)" />
</include>
<group if="$(arg gui)">
<include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
</group>
<node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />
<!-- launch robot control node for moveit motion planning -->
<node pkg="panda_simulation" type="robot_control_node" name="robot_control_node" output="screen" />
<!-- load (not start!) custom joint position controller -->
<node pkg="controller_manager" type="spawner" name="joint_position_launcher" args="--stopped joint_position_controller" />
<!-- run custom node for automatic intialization -->
<node pkg="panda_simulation" type="robot_state_initializer_node" name="robot_state_initializer_node" />
<node pkg="panda_simulation" type="move_group_interface_tutorial" name="move_group_interface_tutorial" respawn="false" output="screen">
</node>
</launch>
...@@ -59,7 +59,9 @@ ...@@ -59,7 +59,9 @@
<!--<node name="move_group_interface_tutorial" pkg="panda_simulation" type="move_group_interface_tutorial" respawn="false" output="screen"/>--> <!--<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="SafetyAwarePlanner" pkg="panda_simulation" type="SafetyAwarePlanner" respawn="false" output="screen">
<param name="message_redirect_rate" type="int" value="100" />
</node>
<!--<node name="motion_planning_api_tutorial" pkg="panda_simulation" type="motion_planning_api_tutorial" 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"/> <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
......
...@@ -59,6 +59,6 @@ ...@@ -59,6 +59,6 @@
<node name="TimedCartesianPlanner" pkg="panda_simulation" type="TimedCartesianPlanner" respawn="false" output="screen"/> <node name="TimedCartesianPlanner" pkg="panda_simulation" type="TimedCartesianPlanner" respawn="false" output="screen"/>
<node name="MqttToRosNode" pkg="panda_simulation" type="MqttToRosNode" respawn="false" output="screen"/> <node name="MqttToRosNode" pkg="panda_simulation" type="MqttToRosNode" respawn="false" output="screen"/>
<node name="RobotStateProvider" pkg="panda_simulation" type="RobotStateProvider"/> <node name="RobotStateProvider" pkg="panda_simulation" type="RobotStateProvider" respawn="false" output="screen"/>
</launch> </launch>
...@@ -5,8 +5,8 @@ ...@@ -5,8 +5,8 @@
#include <gazebo_msgs/LinkStates.h> #include <gazebo_msgs/LinkStates.h>
#include <mqtt/client.h> #include <mqtt/client.h>
#include "string.h" #include "string.h"
#include "robotconfig.pb.h" #include "messages/robotconfig.pb.h"
#include "dataconfig.pb.h" #include "messages/dataconfig.pb.h"
#include "mem_persistence.cpp" #include "mem_persistence.cpp"
using namespace std; using namespace std;
......
...@@ -7,8 +7,8 @@ ...@@ -7,8 +7,8 @@
#include "ros/ros.h" #include "ros/ros.h"
#include "robotconfig.pb.h" #include "messages/robotconfig.pb.h"
#include "dataconfig.pb.h" #include "messages/dataconfig.pb.h"
using namespace std; using namespace std;
using namespace std::chrono; using namespace std::chrono;
...@@ -16,9 +16,8 @@ using namespace std::chrono; ...@@ -16,9 +16,8 @@ using namespace std::chrono;
const string SERVER_ADDRESS { "tcp://localhost:1883" }; const string SERVER_ADDRESS { "tcp://localhost:1883" };
const string CLIENT_ID { "ros_mqtt_consumer" }; const string CLIENT_ID { "ros_mqtt_consumer" };
// --------------------------------------------------------------------------
// Simple function to manually reconnect a client.
// Simple function to manually reconnect a client.
bool try_reconnect(mqtt::client& cli) bool try_reconnect(mqtt::client& cli)
{ {
constexpr int N_ATTEMPT = 30; constexpr int N_ATTEMPT = 30;
...@@ -35,8 +34,6 @@ bool try_reconnect(mqtt::client& cli) ...@@ -35,8 +34,6 @@ bool try_reconnect(mqtt::client& cli)
return false; return false;
} }
/////////////////////////////////////////////////////////////////////////////
void handleRobotConfig(config::RobotConfig robotConfig, ros::NodeHandle n) void handleRobotConfig(config::RobotConfig robotConfig, ros::NodeHandle n)
{ {
std::cout << "Retrieved new target-speed: " << robotConfig.speed() << std::endl; std::cout << "Retrieved new target-speed: " << robotConfig.speed() << std::endl;
......
...@@ -5,7 +5,7 @@ ...@@ -5,7 +5,7 @@
#include <gazebo_msgs/LinkStates.h> #include <gazebo_msgs/LinkStates.h>
#include <mqtt/client.h> #include <mqtt/client.h>
#include "string.h" #include "string.h"
#include "linkstate.pb.h" #include "messages/linkstate.pb.h"
#include "mem_persistence.cpp" #include "mem_persistence.cpp"
bool export_to_log = false; bool export_to_log = false;
......
...@@ -7,7 +7,7 @@ ...@@ -7,7 +7,7 @@
int main(int argc, char** argv) int main(int argc, char** argv)
{ {
ros::init(argc, argv, "CONSTRAINT PLANNER"); ros::init(argc, argv, "Safety-aware PLANNER");
ros::NodeHandle node_handle; ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1); ros::AsyncSpinner spinner(1);
spinner.start(); spinner.start();
...@@ -27,15 +27,15 @@ int main(int argc, char** argv) ...@@ -27,15 +27,15 @@ int main(int argc, char** argv)
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.75; text_pose.translation().z() = 1.75;
visual_tools.publishText(text_pose, "Sample constraint planner node", rvt::WHITE, rvt::XLARGE); visual_tools.publishText(text_pose, "Safety-aware planner node", rvt::WHITE, rvt::XLARGE);
// Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
visual_tools.trigger(); visual_tools.trigger();
// Getting Basic Information // Getting Basic Information
ROS_INFO_NAMED("constraint_planner", "Planning frame: %s", move_group.getPlanningFrame().c_str()); ROS_INFO_NAMED("safety_aware_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("safety_aware_planner", "End effector link: %s", move_group.getEndEffectorLink().c_str());
ROS_INFO_NAMED("constraint_planner", "Available Planning Groups:"); ROS_INFO_NAMED("safety_aware_planner", "Available Planning Groups:");
std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(), std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
std::ostream_iterator<std::string>(std::cout, ", ")); std::ostream_iterator<std::string>(std::cout, ", "));
...@@ -64,7 +64,7 @@ int main(int argc, char** argv) ...@@ -64,7 +64,7 @@ int main(int argc, char** argv)
//visual_tools.processCollisionObjectMsg(collision_objects[2], rviz_visual_tools::RED); //visual_tools.processCollisionObjectMsg(collision_objects[2], rviz_visual_tools::RED);
// visual_tools.setAlpha(0.0); // visual_tools.setAlpha(0.0);
// Now, let's add the collision object into the world // Now, let's add the collision object into the world
ROS_INFO_NAMED("tutorial", "Add an object into the world"); ROS_INFO_NAMED("safety_aware_planner", "Add an object into the world");
planning_scene_interface.addCollisionObjects(collision_objects); planning_scene_interface.addCollisionObjects(collision_objects);
...@@ -91,7 +91,7 @@ int main(int argc, char** argv) ...@@ -91,7 +91,7 @@ int main(int argc, char** argv)
moveit::planning_interface::MoveGroupInterface::Plan my_plan; moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("constraint_planner", "Visualizing constraint plan %s", success ? "" : "FAILED"); ROS_INFO_NAMED("safety_aware_planner", "Visualizing constraint plan %s", success ? "" : "FAILED");
visual_tools.prompt("Press 'next' to move the simulated robot."); visual_tools.prompt("Press 'next' to move the simulated robot.");
visual_tools.trigger(); visual_tools.trigger();
......
...@@ -2,7 +2,6 @@ ...@@ -2,7 +2,6 @@
// Created by sebastian on 31.03.20. // Created by sebastian on 31.03.20.
// //
#include <moveit/move_group_interface/move_group_interface.h> #include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/DisplayTrajectory.h> #include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/CollisionObject.h> #include <moveit_msgs/CollisionObject.h>
...@@ -17,8 +16,7 @@ const double default_velocity = 1.0; ...@@ -17,8 +16,7 @@ const double default_velocity = 1.0;
bool isInitialized = false; bool isInitialized = false;
void initRawTrajectory(ros::NodeHandle node_handle, moveit::planning_interface::MoveGroupInterface* group) void initRawTrajectory(ros::NodeHandle node_handle, moveit::planning_interface::MoveGroupInterface *group) {
{
geometry_msgs::Pose target_pose_1 = group->getCurrentPose().pose; geometry_msgs::Pose target_pose_1 = group->getCurrentPose().pose;
geometry_msgs::Pose target_pose_2 = group->getCurrentPose().pose; geometry_msgs::Pose target_pose_2 = group->getCurrentPose().pose;
geometry_msgs::Pose target_pose_3 = group->getCurrentPose().pose; geometry_msgs::Pose target_pose_3 = group->getCurrentPose().pose;
...@@ -45,8 +43,7 @@ void initRawTrajectory(ros::NodeHandle node_handle, moveit::planning_interface:: ...@@ -45,8 +43,7 @@ void initRawTrajectory(ros::NodeHandle node_handle, moveit::planning_interface::
raw_trajectory.push_back(target_pose_4); raw_trajectory.push_back(target_pose_4);
} }
void moveRobotToInitialState(ros::NodeHandle node_handle) void moveRobotToInitialState(ros::NodeHandle node_handle) {
{
ROS_INFO("Moving to initial pose of trajectory."); ROS_INFO("Moving to initial pose of trajectory.");
moveit::planning_interface::MoveGroupInterface group("panda_arm"); moveit::planning_interface::MoveGroupInterface group("panda_arm");
...@@ -71,8 +68,29 @@ void moveRobotToInitialState(ros::NodeHandle node_handle) ...@@ -71,8 +68,29 @@ void moveRobotToInitialState(ros::NodeHandle node_handle)
group.execute(cartesian_plan); group.execute(cartesian_plan);
} }
int main(int argc, char **argv) void computeTrajectory(moveit::planning_interface::MoveGroupInterface &group, int i,
{ moveit::planning_interface::MoveGroupInterface::Plan &plan,
moveit_msgs::RobotTrajectory &trajectory_msg) {
std::vector<geometry_msgs::Pose> single_traj;
single_traj.push_back(raw_trajectory.at(i));
group.setStartStateToCurrentState();
group.setPlanningTime(10.0);
double fraction = group.computeCartesianPath(single_traj, 0.01, 0.0, trajectory_msg, false);
robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "panda_arm");
rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg);
trajectory_processing::IterativeParabolicTimeParameterization iptp;
bool success = iptp.computeTimeStamps(rt);
ROS_INFO("Computed time stamp %s", success ? "SUCCEDED" : "FAILED");
rt.getRobotTrajectoryMsg(trajectory_msg);
plan.trajectory_ = trajectory_msg;
ROS_INFO("Visualizing cartesian plan (%.2f%% acheived)", fraction * 100.0);
}
int main(int argc, char **argv) {
// setup this ros-node
ros::init(argc, argv, "timed_cartesian_planner"); ros::init(argc, argv, "timed_cartesian_planner");
ros::NodeHandle node_handle; ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1); ros::AsyncSpinner spinner(1);
...@@ -91,7 +109,7 @@ int main(int argc, char **argv) ...@@ -91,7 +109,7 @@ int main(int argc, char **argv)
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.75; text_pose.translation().z() = 1.75;
visual_tools.publishText(text_pose, "Sample constraint planner node", rvt::WHITE, rvt::XLARGE); visual_tools.publishText(text_pose, "cartesian planner node", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger(); visual_tools.trigger();
// Initialize start state of robot and target trajectory. // Initialize start state of robot and target trajectory.
...@@ -99,69 +117,42 @@ int main(int argc, char **argv) ...@@ -99,69 +117,42 @@ int main(int argc, char **argv)
moveRobotToInitialState(node_handle); moveRobotToInitialState(node_handle);
initRawTrajectory(node_handle, &group); initRawTrajectory(node_handle, &group);
for(int i = 0; i < raw_trajectory.size(); i++) // execute the trajectory which consists of single waypoints to allow distinct planning
{ for (int i = 0; i < raw_trajectory.size(); i++) {
double velocity = 0.0; double velocity = 0.0;
if(!node_handle.getParam("robot_speed_factor", velocity)) if (!node_handle.getParam("robot_speed_factor", velocity)) {
{
velocity = default_velocity; velocity = default_velocity;
} }
std::cout << ">>>>>>>>>>>>>>>>> starting planing step " << i << " (speed: " << velocity << ")" << std::endl; std::cout << ">>>>>>>>>>>>>>>>> starting planing step " << i << " (speed: " << velocity << ")" << std::endl;
std::vector<geometry_msgs::Pose> single_traj; // compute the cartesian trajectory
single_traj.push_back(raw_trajectory.at(i));
moveit::planning_interface::MoveGroupInterface group("panda_arm");
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
moveit::planning_interface::MoveGroupInterface::Plan plan; moveit::planning_interface::MoveGroupInterface::Plan plan;
group.setStartStateToCurrentState();
moveit_msgs::RobotTrajectory trajectory_msg; moveit_msgs::RobotTrajectory trajectory_msg;
group.setPlanningTime(10.0); computeTrajectory(group, i, plan, trajectory_msg);
double fraction = group.computeCartesianPath(single_traj,0.01,0.0,trajectory_msg, false);
robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "panda_arm");
rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg);
// Third create a IterativeParabolicTimeParameterization object
trajectory_processing::IterativeParabolicTimeParameterization iptp;
// Fourth compute computeTimeStamps
bool success = iptp.computeTimeStamps(rt);
ROS_INFO("Computed time stamp %s",success ? "SUCCEDED":"FAILED");
rt.getRobotTrajectoryMsg(trajectory_msg);
plan.trajectory_ = trajectory_msg;
ROS_INFO("Visualizing cartesian plan (%.2f%% acheived)",fraction * 100.0);
//std::cout << ">>>>>>>>>>>>>>>>> data before: " << trajectory_msg << std::endl; // adjust velocities, accelerations (= sqrt of velocity) and time_from_start
// adjust velocities, accelerations and time_from_start for (int i = 0; i < trajectory_msg.joint_trajectory.points.size(); i++) {
for(int i = 0; i < trajectory_msg.joint_trajectory.points.size(); i++)
{
double new_tfs = 1 / velocity; double new_tfs = 1 / velocity;
trajectory_msg.joint_trajectory.points[i].time_from_start.operator*=(new_tfs); trajectory_msg.joint_trajectory.points[i].time_from_start.operator*=(new_tfs);
for(int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).velocities.size(); j++) for (int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).velocities.size(); j++) {
{
if (trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) != 0.0) { if (trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) != 0.0) {
trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) *= velocity; trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) *= velocity;
} }
} }
for(int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).accelerations.size(); j++) for (int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).accelerations.size(); j++) {
{
trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j) = trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j) =
sqrt(std::abs(trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j))); sqrt(std::abs(trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j)));
} }
} }
//std::cout << ">>>>>>>>>>>>>>>>> data after: " << trajectory_msg << std::endl;
// execute one step of the trajectory
group.execute(plan); group.execute(plan);
if(i == (raw_trajectory.size() -1)) // make sure the robot moves in an infinite circle
{ if (i == (raw_trajectory.size() - 1)) {
i = -1; i = -1;
} }
} }
......
#include <gazebo_msgs/SetModelState.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
static const std::string PLANNING_GROUP_ARM = "panda_arm";
static const double PANDA_ARM_TO_HAND_OFFSET = 0.12;
static const double PANDA_HAND_TO_FINGER_OFFSET = 0.04;
ros::Publisher gazebo_model_state_pub;
robot_model::RobotModelPtr kinematic_model;
robot_state::RobotStatePtr kinematic_state;
void jointStatesCallback(const sensor_msgs::JointState &joint_states_current)
{
const robot_state::JointModelGroup *joint_model_group = kinematic_model->getJointModelGroup("panda_arm");
const std::vector<std::string> &joint_names = joint_model_group->getJointModelNames();
std::vector<double> joint_states;
for (size_t i = 0; i < joint_states_current.position.size() - 2; ++i)
{
joint_states.push_back(joint_states_current.position[i + 2]);
}
kinematic_state->setJointGroupPositions(joint_model_group, joint_states);
const Eigen::Affine3d &end_effector_state = kinematic_state->getGlobalLinkTransform("panda_link8");
double end_effector_z_offset = PANDA_ARM_TO_HAND_OFFSET + PANDA_HAND_TO_FINGER_OFFSET;
Eigen::Affine3d tmp_transform(Eigen::Translation3d(Eigen::Vector3d(0.0, 0.0, end_effector_z_offset)));
Eigen::Affine3d newState = end_effector_state * tmp_transform;
geometry_msgs::Pose pose;
pose.position.x = newState.translation().x();
pose.position.y = newState.translation().y();
pose.position.z = newState.translation().z();
Eigen::Quaterniond quat(newState.rotation());
pose.orientation.w = quat.w();
pose.orientation.x = quat.x();
pose.orientation.y = quat.y();
pose.orientation.z = quat.z();
ROS_DEBUG_STREAM("translation" << std::endl << newState.translation());
ROS_DEBUG_STREAM("rotation" << std::endl << newState.rotation());
gazebo_msgs::ModelState model_state;
// This string results from the spawn_urdf call in the box.launch file argument: -model box
model_state.model_name = std::string("box");
model_state.pose = pose;
model_state.reference_frame = std::string("world");
gazebo_model_state_pub.publish(model_state);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "box_publisher_node");
ros::NodeHandle node_handle;
ros::Subscriber joint_states_sub = node_handle.subscribe("/joint_states", 1, jointStatesCallback);
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
kinematic_model = robot_model_loader.getModel();
kinematic_state = robot_state::RobotStatePtr(new robot_state::RobotState(kinematic_model));
gazebo_model_state_pub = node_handle.advertise<gazebo_msgs::ModelState>("/gazebo/set_model_state", 1);
ros::spin();
return 0;
}
\ No newline at end of file
File moved
File moved
This diff is collapsed.
This diff is collapsed.
File moved
File moved
...@@ -15,97 +15,6 @@ ...@@ -15,97 +15,6 @@
#include <trajectory_msgs/JointTrajectoryPoint.h> #include <trajectory_msgs/JointTrajectoryPoint.h>
int main(int argc, char** argv) int main(int argc, char** argv)
{
ros::init(argc, argv, "move_group_interface_tutorial");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
std::cout << ">>>>>>>>>>>>>>>>> SLEEPING <<<<<<<<<<<<<<<< " << std::endl;
ros::Duration(5.0).sleep();
std::cout << ">>>>>>>>>>>>>>>>> WAKING UP <<<<<<<<<<<<<<<< " << std::endl;
//moveit::planning_interface::MoveGroup group("arm_gp");
moveit::planning_interface::MoveGroupInterface group("panda_arm");
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
moveit_msgs::DisplayTrajectory display_trajectory;
moveit::planning_interface::MoveGroupInterface::Plan plan;
group.setStartStateToCurrentState();
std::vector<geometry_msgs::Pose> waypoints;
geometry_msgs::Pose target_pose = group.getCurrentPose().pose;
target_pose.position.x += 2.0;
target_pose.position.y += 5.0;
target_pose.position.z -= 2.0;
waypoints.push_back(target_pose);
/*target_pose.position.y -= 0.0;
waypoints.push_back(target_pose);
target_pose.position.z -= 0.08;
target_pose.position.y += 0.0;
target_pose.position.x -= 0.0;
waypoints.push_back(target_pose);*/
moveit_msgs::RobotTrajectory trajectory_msg;
group.setPlanningTime(10.0);
double fraction = group.computeCartesianPath(waypoints,
0.01, // eef_step
0.0, // jump_threshold
trajectory_msg, false);
// The trajectory needs to be modified so it will include velocities as well.
// First to create a RobotTrajectory object
robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "panda_arm");
// Second get a RobotTrajectory from trajectory
rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg);
// Third create a IterativeParabolicTimeParameterization object
trajectory_processing::IterativeParabolicTimeParameterization iptp;
// Fourth compute computeTimeStamps
bool success = iptp.computeTimeStamps(rt);
ROS_INFO("Computed time stamp %s",success?"SUCCEDED":"FAILED");
// Get RobotTrajectory_msg from RobotTrajectory
rt.getRobotTrajectoryMsg(trajectory_msg);
// Check trajectory_msg for velocities not empty
//std::cout << ">>>>>>>>>>>>>>>>> data before: " << trajectory_msg << std::endl;
// adjust velocities, accelerations and time_from_start
for(int i = 0; i < trajectory_msg.joint_trajectory.points.size(); i++)
{
trajectory_msg.joint_trajectory.points[i].time_from_start.operator*=(5);
for(int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).velocities.size(); j++)
{
if(trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) != 0.0){
trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) /= 5;
//trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) = 0;
}
}
for(int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).accelerations.size(); j++)
{
trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j) =
sqrt(std::abs(trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j)));
}
}
//std::cout << ">>>>>>>>>>>>>>>>> data after: " << trajectory_msg << std::endl;
plan.trajectory_ = trajectory_msg;
ROS_INFO("Visualizing plan 4 (cartesian path) (%.2f%% acheived)",fraction * 100.0);
sleep(5.0);
group.execute(plan);
ros::shutdown();
return 0;
}
/*int main(int argc, char** argv)
{ {
ros::init(argc, argv, "CONSTRAINT PLANNER"); ros::init(argc, argv, "CONSTRAINT PLANNER");
ros::NodeHandle node_handle; ros::NodeHandle node_handle;
...@@ -197,4 +106,4 @@ int main(int argc, char **argv) ...@@ -197,4 +106,4 @@ int main(int argc, char **argv)
ros::shutdown(); ros::shutdown();
return 0; return 0;
}*/ }
\ No newline at end of file \ No newline at end of file
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2012, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Sachin Chitta, Michael Lautman */
#include <pluginlib/class_loader.h>
#include <ros/ros.h>
// MoveIt
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_interface/planning_interface.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/PlanningScene.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <boost/scoped_ptr.hpp>
int main(int argc, char** argv)
{
const std::string node_name = "motion_planning_tutorial";
ros::init(argc, argv, node_name);
ros::AsyncSpinner spinner(1);
spinner.start();
ros::NodeHandle node_handle("~");
// BEGIN_TUTORIAL
// Start
// ^^^^^
// Setting up to start using a planner is pretty easy. Planners are
// setup as plugins in MoveIt and you can use the ROS pluginlib
// interface to load any planner that you want to use. Before we
// can load the planner, we need two objects, a RobotModel and a
// PlanningScene. We will start by instantiating a `RobotModelLoader`_
// object, which will look up the robot description on the ROS
// parameter server and construct a :moveit_core:`RobotModel` for us
// to use.
//
// .. _RobotModelLoader:
// http://docs.ros.org/indigo/api/moveit_ros_planning/html/classrobot__model__loader_1_1RobotModelLoader.html
const std::string PLANNING_GROUP = "panda_arm";
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();
/* Create a RobotState and JointModelGroup to keep track of the current robot pose and planning group*/
robot_state::RobotStatePtr robot_state(new robot_state::RobotState(robot_model));
const robot_state::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP);
// Using the :moveit_core:`RobotModel`, we can construct a :planning_scene:`PlanningScene`
// that maintains the state of the world (including the robot).
planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));
// Configure a valid robot state
planning_scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "ready");
// We will now construct a loader to load a planner, by name.
// Note that we are using the ROS pluginlib library here.
boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager>> planner_plugin_loader;
planning_interface::PlannerManagerPtr planner_instance;
std::string planner_plugin_name;
// We will get the name of planning plugin we want to load
// from the ROS parameter server, and then load the planner
// making sure to catch all exceptions.
if (!node_handle.getParam("planning_plugin", planner_plugin_name))
ROS_FATAL_STREAM("Could not find planner plugin name");
try
{
planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>(
"moveit_core", "planning_interface::PlannerManager"));
}
catch (pluginlib::PluginlibException& ex)
{
ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what());
}
try
{
planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name));
if (!planner_instance->initialize(robot_model, node_handle.getNamespace()))
ROS_FATAL_STREAM("Could not initialize planner instance");
ROS_INFO_STREAM("Using planning interface '" << planner_instance->getDescription() << "'");
}
catch (pluginlib::PluginlibException& ex)
{
const std::vector<std::string>& classes = planner_plugin_loader->getDeclaredClasses();
std::stringstream ss;
for (std::size_t i = 0; i < classes.size(); ++i)
ss << classes[i] << " ";
ROS_ERROR_STREAM("Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl
<< "Available plugins: " << ss.str());
}
// Visualization
// ^^^^^^^^^^^^^
// The package MoveItVisualTools provides many capabilities for visualizing objects, robots,
// and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script.
namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
visual_tools.loadRobotStatePub("/display_robot_state");
visual_tools.enableBatchPublishing();
visual_tools.deleteAllMarkers(); // clear all old markers
visual_tools.trigger();
/* Remote control is an introspection tool that allows users to step through a high level script
via buttons and keyboard shortcuts in RViz */
visual_tools.loadRemoteControl();
/* RViz provides many types of markers, in this demo we will use text, cylinders, and spheres*/
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.75;
visual_tools.publishText(text_pose, "Motion Planning API Demo", rvt::WHITE, rvt::XLARGE);
/* Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations */
visual_tools.trigger();
/* We can also use visual_tools to wait for user input */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");
// Pose Goal
// ^^^^^^^^^
// We will now create a motion plan request for the arm of the Panda
// specifying the desired pose of the end-effector as input.
visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN);
visual_tools.trigger();
planning_interface::MotionPlanRequest req;
planning_interface::MotionPlanResponse res;
geometry_msgs::PoseStamped pose;
pose.header.frame_id = "panda_link0";
pose.pose.position.x = 0.3;
pose.pose.position.y = 0.4;
pose.pose.position.z = 0.75;
pose.pose.orientation.w = 1.0;
// A tolerance of 0.01 m is specified in position
// and 0.01 radians in orientation
std::vector<double> tolerance_pose(3, 0.01);
std::vector<double> tolerance_angle(3, 0.01);
// We will create the request as a constraint using a helper function available
// from the
// `kinematic_constraints`_
// package.
//
// .. _kinematic_constraints:
// http://docs.ros.org/indigo/api/moveit_core/html/namespacekinematic__constraints.html#a88becba14be9ced36fefc7980271e132
moveit_msgs::Constraints pose_goal =
kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle);
req.group_name = PLANNING_GROUP;
req.goal_constraints.push_back(pose_goal);
// We now construct a planning context that encapsulate the scene,
// the request and the response. We call the planner using this
// planning context
planning_interface::PlanningContextPtr context =
planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
context->solve(res);
if (res.error_code_.val != res.error_code_.SUCCESS)
{
ROS_ERROR("Could not compute plan successfully");
return 0;
}
// Visualize the result
// ^^^^^^^^^^^^^^^^^^^^
ros::Publisher display_publisher =
node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
moveit_msgs::DisplayTrajectory display_trajectory;
/* Visualize the trajectory */
moveit_msgs::MotionPlanResponse response;
res.getMessage(response);
display_trajectory.trajectory_start = response.trajectory_start;
display_trajectory.trajectory.push_back(response.trajectory);
visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
visual_tools.trigger();
display_publisher.publish(display_trajectory);
/* Set the state in the planning scene to the final state of the last plan */
robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
planning_scene->setCurrentState(*robot_state.get());
// Display the goal state
visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN);
visual_tools.publishAxisLabeled(pose.pose, "goal_1");
visual_tools.publishText(text_pose, "Pose Goal (1)", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
/* We can also use visual_tools to wait for user input */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
// Joint Space Goals
// ^^^^^^^^^^^^^^^^^
// Now, setup a joint space goal
robot_state::RobotState goal_state(robot_model);
std::vector<double> joint_values = { -1.0, 0.7, 0.7, -1.5, -0.7, 2.0, 0.0 };
goal_state.setJointGroupPositions(joint_model_group, joint_values);
moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group);
req.goal_constraints.clear();
req.goal_constraints.push_back(joint_goal);
// Call the planner and visualize the trajectory
/* Re-construct the planning context */
context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
/* Call the Planner */
context->solve(res);
/* Check that the planning was successful */
if (res.error_code_.val != res.error_code_.SUCCESS)
{
ROS_ERROR("Could not compute plan successfully");
return 0;
}
/* Visualize the trajectory */
res.getMessage(response);
display_trajectory.trajectory.push_back(response.trajectory);
/* Now you should see two planned trajectories in series*/
visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
visual_tools.trigger();
display_publisher.publish(display_trajectory);
/* We will add more goals. But first, set the state in the planning
scene to the final state of the last plan */
robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
planning_scene->setCurrentState(*robot_state.get());
// Display the goal state
visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN);
visual_tools.publishAxisLabeled(pose.pose, "goal_2");
visual_tools.publishText(text_pose, "Joint Space Goal (2)", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
/* Wait for user input */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
/* Now, we go back to the first goal to prepare for orientation constrained planning */
req.goal_constraints.clear();
req.goal_constraints.push_back(pose_goal);
context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
context->solve(res);
res.getMessage(response);
display_trajectory.trajectory.push_back(response.trajectory);
visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
visual_tools.trigger();
display_publisher.publish(display_trajectory);
/* Set the state in the planning scene to the final state of the last plan */
robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
planning_scene->setCurrentState(*robot_state.get());
// Display the goal state
visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN);
visual_tools.trigger();
/* Wait for user input */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
// Adding Path Constraints
// ^^^^^^^^^^^^^^^^^^^^^^^
// Let's add a new pose goal again. This time we will also add a path constraint to the motion.
/* Let's create a new pose goal */
pose.pose.position.x = 0.32;
pose.pose.position.y = -0.25;
pose.pose.position.z = 0.65;
pose.pose.orientation.w = 1.0;
moveit_msgs::Constraints pose_goal_2 =
kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle);
/* Now, let's try to move to this new pose goal*/
req.goal_constraints.clear();
req.goal_constraints.push_back(pose_goal_2);
/* But, let's impose a path constraint on the motion.
Here, we are asking for the end-effector to stay level*/
geometry_msgs::QuaternionStamped quaternion;
quaternion.header.frame_id = "panda_link0";
quaternion.quaternion.w = 1.0;
req.path_constraints = kinematic_constraints::constructGoalConstraints("panda_link8", quaternion);
// Imposing path constraints requires the planner to reason in the space of possible positions of the end-effector
// (the workspace of the robot)
// because of this, we need to specify a bound for the allowed planning volume as well;
// Note: a default bound is automatically filled by the WorkspaceBounds request adapter (part of the OMPL pipeline,
// but that is not being used in this example).
// We use a bound that definitely includes the reachable space for the arm. This is fine because sampling is not done
// in this volume
// when planning for the arm; the bounds are only used to determine if the sampled configurations are valid.
req.workspace_parameters.min_corner.x = req.workspace_parameters.min_corner.y =
req.workspace_parameters.min_corner.z = -2.0;
req.workspace_parameters.max_corner.x = req.workspace_parameters.max_corner.y =
req.workspace_parameters.max_corner.z = 2.0;
// Call the planner and visualize all the plans created so far.
context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
context->solve(res);
res.getMessage(response);
display_trajectory.trajectory.push_back(response.trajectory);
visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
visual_tools.trigger();
display_publisher.publish(display_trajectory);
/* Set the state in the planning scene to the final state of the last plan */
robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
planning_scene->setCurrentState(*robot_state.get());
// Display the goal state
visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN);
visual_tools.publishAxisLabeled(pose.pose, "goal_3");
visual_tools.publishText(text_pose, "Orientation Constrained Motion Plan (3)", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
// END_TUTORIAL
/* Wait for user input */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to exit the demo");
planner_instance.reset();
return 0;
}
...@@ -234,6 +234,9 @@ int main(int argc, char** argv) ...@@ -234,6 +234,9 @@ int main(int argc, char** argv)
move_group.setPlanningTime(10.0); move_group.setPlanningTime(10.0);
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
move_group.move();
ROS_INFO_NAMED("tutorial", "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED"); ROS_INFO_NAMED("tutorial", "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED");
// Visualize the plan in RViz // Visualize the plan in RViz
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment