From 387349027c5b1eacbb7b46517428599ef662810f Mon Sep 17 00:00:00 2001
From: SebastianEbert <sebastian.ebert@tu-dresden.de>
Date: Thu, 2 Apr 2020 09:42:22 +0200
Subject: [PATCH] velocity constraint planning, retreival and logging of robot
state
---
CMakeLists.txt | 14 ++--
launch/simulation_rosrag.launch | 63 ++++++++++++++++
src/RobotStateProvider.cpp | 124 ++++++++++++++++++++++++++++++++
src/TimedCartesianPlanner.cpp | 120 +++++++++++++++++++++++++++++++
4 files changed, 316 insertions(+), 5 deletions(-)
create mode 100644 launch/simulation_rosrag.launch
create mode 100644 src/RobotStateProvider.cpp
create mode 100644 src/TimedCartesianPlanner.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4835386..a88278c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -12,6 +12,7 @@ find_package(catkin REQUIRED
COMPONENTS controller_manager
effort_controllers
gazebo_ros
+ genmsg
joint_state_controller
joint_state_publisher
joint_trajectory_controller
@@ -45,7 +46,8 @@ catkin_package(CATKIN_DEPENDS
controller_interface
hardware_interface
pluginlib
-# DEPENDS
+ #DEPENDS
+ #libpaho-mqtt
# system_lib
)
@@ -58,20 +60,22 @@ include_directories(${catkin_INCLUDE_DIRS})
# add custom controller as library
add_library(${PROJECT_NAME}_controllers_lib src/joint_position_controller.cpp)
-#add_library(SafetyEnvironmentCreator src/SafetyEnvironmentCreator.cpp)
# Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}_controllers_lib ${catkin_LIBRARIES})
-#target_link_libraries(SafetyEnvironmentCreator ${catkin_LIBRARIES} ${Boost_LIBRARIES})
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(SafetyAwarePlanner src/SafetyAwarePlanner.cpp src/SafetyEnvironmentCreator.cpp)
+add_executable(SampleConstraintPlanner src/SampleConstraintPlanner.cpp)
+add_executable(RobotStateProvider src/RobotStateProvider.cpp)
+add_executable(TimedCartesianPlanner src/TimedCartesianPlanner.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(SafetyAwarePlanner ${catkin_LIBRARIES})
-#target_link_libraries(constraint_planner ${catkin_LIBRARIES} ${Boost_LIBRARIES} SafetyEnvironmentCreator)
-
+target_link_libraries(SampleConstraintPlanner ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+target_link_libraries(RobotStateProvider ${catkin_LIBRARIES})
+target_link_libraries(TimedCartesianPlanner ${catkin_LIBRARIES})
\ No newline at end of file
diff --git a/launch/simulation_rosrag.launch b/launch/simulation_rosrag.launch
new file mode 100644
index 0000000..91e8184
--- /dev/null
+++ b/launch/simulation_rosrag.launch
@@ -0,0 +1,63 @@
+<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 name="TimedCartesianPlanner" pkg="panda_simulation" type="TimedCartesianPlanner" respawn="false" output="screen"/>
+ <node name="RobotStateProvider" pkg="panda_simulation" type="RobotStateProvider"/>
+
+</launch>
diff --git a/src/RobotStateProvider.cpp b/src/RobotStateProvider.cpp
new file mode 100644
index 0000000..33fc60e
--- /dev/null
+++ b/src/RobotStateProvider.cpp
@@ -0,0 +1,124 @@
+//
+// Created by sebastian on 31.03.20.
+//
+#include "ros/ros.h"
+#include "std_msgs/String.h"
+#include <gazebo_msgs/LinkStates.h>
+#include <MQTTClient.h>
+
+// CONFIGURATION START
+// ^^^^^^^^^^^^^^^^^^^
+
+// configure the number of not redirected messages between redirected messages
+const int message_redirect_rate = 100;
+
+// values to export
+bool export_position = true;
+bool export_orientation = true;
+bool export_twist_linear = true;
+bool export_twist_angular = true;
+bool export_names = true;
+
+// CONFIGURATION END
+// ^^^^^^^^^^^^^^^^^
+
+int current_redirect = 0;
+bool isInitialized = false;
+
+/*
+ * logs to its own file in /.ros/logs (configured in launch-file)
+ */
+void exportMessageToLog(const gazebo_msgs::LinkStates & msg)
+{
+ if(export_names){
+
+ ROS_INFO_STREAM("<<< LINK NAMES >>>");
+
+ for(int i = 0; i < msg.name.size(); i++)
+ {
+ ROS_INFO_STREAM("[" << i << "] " << msg.name.at(i));
+ }
+ }
+
+ if(export_position){
+
+ ROS_INFO_STREAM("<<< POSITIONS BY LINK >>>");
+
+ for(int i = 0; i < msg.pose.size(); i++)
+ {
+ ROS_INFO_STREAM("[" << i << "] [x] " << msg.pose.at(i).position.x);
+ ROS_INFO_STREAM("[" << i << "] [y] " << msg.pose.at(i).position.y);
+ ROS_INFO_STREAM("[" << i << "] [z] " << msg.pose.at(i).position.z);
+ }
+ }
+
+ if(export_orientation){
+
+ ROS_INFO_STREAM("<<< ORIENTATIONS BY LINK >>>");
+
+ for(int i = 0; i < msg.pose.size(); i++)
+ {
+ ROS_INFO_STREAM("[" << i << "] [w] " << msg.pose.at(i).orientation.w);
+ ROS_INFO_STREAM("[" << i << "] [x] " << msg.pose.at(i).orientation.x);
+ ROS_INFO_STREAM("[" << i << "] [y] " << msg.pose.at(i).orientation.y);
+ ROS_INFO_STREAM("[" << i << "] [z] " << msg.pose.at(i).orientation.z);
+ }
+ }
+
+ if(export_twist_linear){
+ ROS_INFO_STREAM("<<< LINEAR TWISTS BY LINK >>>");
+ for(int i = 0; i < msg.twist.size(); i++)
+ {
+ ROS_INFO_STREAM("[" << i << "] [x] " << msg.twist.at(i).linear.x);
+ ROS_INFO_STREAM("[" << i << "] [y] " << msg.twist.at(i).linear.y);
+ ROS_INFO_STREAM("[" << i << "] [z] " << msg.twist.at(i).linear.z);
+ }
+ }
+
+ if(export_twist_angular){
+ ROS_INFO_STREAM("<<< ANGULAR TWISTS BY LINK >>>");
+ for(int i = 0; i < msg.twist.size(); i++)
+ {
+ ROS_INFO_STREAM("[" << i << "] [x] " << msg.twist.at(i).angular.x);
+ ROS_INFO_STREAM("[" << i << "] [y] " << msg.twist.at(i).angular.y);
+ ROS_INFO_STREAM("[" << i << "] [z] " << msg.twist.at(i).angular.z);
+ }
+ }
+}
+
+void providerCallback(const gazebo_msgs::LinkStates & msg)
+{
+ if(current_redirect != 0 && current_redirect != message_redirect_rate){
+ //std::cout << "Ignoring message (redirect: " << current_redirect << " )" << std::endl;
+ current_redirect++;
+ return;
+ }
+
+ if(current_redirect == 0 || current_redirect == message_redirect_rate){
+ //std::cout << "Redirecting message:" << msg.pose.at(1).position << std::endl;
+ exportMessageToLog(msg);
+ if(current_redirect == message_redirect_rate){
+ current_redirect = 1;
+ } else{
+ current_redirect++;
+ }
+ }
+}
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "listener");
+ ros::NodeHandle n;
+ n.getParam("ready", isInitialized);
+
+ // make sure the robot is initialized
+ while(!isInitialized){
+ //std::cout << "Waiting" << std::endl;
+ n.getParam("tud_planner_ready", isInitialized);
+ }
+
+ ros::Subscriber sub = n.subscribe("/gazebo/link_states", 10000, providerCallback);
+ ros::spin();
+
+ return 0;
+}
\ No newline at end of file
diff --git a/src/TimedCartesianPlanner.cpp b/src/TimedCartesianPlanner.cpp
new file mode 100644
index 0000000..e2552ec
--- /dev/null
+++ b/src/TimedCartesianPlanner.cpp
@@ -0,0 +1,120 @@
+//
+// Created by sebastian on 31.03.20.
+//
+#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>
+#include <moveit/trajectory_processing/iterative_time_parameterization.h>
+#include <trajectory_msgs/JointTrajectoryPoint.h>
+
+/**
+ * allows time/velocity-constraint planning for cartesian paths
+ */
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "move_group_interface_tutorial");
+ ros::NodeHandle node_handle;
+ ros::AsyncSpinner spinner(1);
+ spinner.start();
+
+ // wait for robot init of robot_state_initializer
+ std::cout << ">>>>>>>>>>>>>>>>> SLEEPING <<<<<<<<<<<<<<<< " << std::endl;
+ ros::Duration(5.0).sleep();
+ std::cout << ">>>>>>>>>>>>>>>>> WAKING UP <<<<<<<<<<<<<<<< " << std::endl;
+ node_handle.setParam("tud_planner_ready", true);
+
+ // 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();
+
+ 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 start_pose = group.getCurrentPose().pose;
+ geometry_msgs::Pose target_pose_1 = group.getCurrentPose().pose;
+
+ target_pose_1.position.z = start_pose.position.z - 0.2;
+ target_pose_1.position.y = start_pose.position.y + 0.2;
+ target_pose_1.position.x = start_pose.position.x + 0.2;
+ waypoints.push_back(target_pose_1);
+
+ geometry_msgs::Pose target_pose_2 = group.getCurrentPose().pose;
+ target_pose_2.position.z = start_pose.position.z + 0.2;
+ target_pose_1.position.y = start_pose.position.y - 0.2;
+ target_pose_2.position.x = start_pose.position.x - 0.2;
+ waypoints.push_back(target_pose_2);
+
+ moveit_msgs::RobotTrajectory trajectory_msg;
+ group.setPlanningTime(10.0);
+
+ double fraction = group.computeCartesianPath(waypoints,0.01,0.0,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");
+ rt.getRobotTrajectoryMsg(trajectory_msg);
+
+ //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*=(2);
+ 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) /= 2;
+ //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);
+
+ group.execute(plan);
+
+ ros::shutdown();
+ return 0;
+}
\ No newline at end of file
--
GitLab