diff --git a/CMakeLists.txt b/CMakeLists.txt index 44d51527bf2e44df231d6755330a53923bd67b46..6218192054eb22f7981b6bbc6013935a04045a32 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -68,6 +68,78 @@ catkin_package(CATKIN_DEPENDS # system_lib ) +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + + # ################################################################################################################################ # Build ## # ################################################################################################################################ @@ -86,7 +158,7 @@ add_library(MqttUtil src/util/MqttUtil.cpp src/util/MqttUtil.h) add_executable(RobotStateProvider src/RobotStateProvider.cpp ${PROTO_SRCS} ${PROTO_HDRS}) -add_executable(TimedPlanner src/TimedPlanner.cpp) +add_executable(TimedPlanner src/TimedPlanner.cpp src/TimedPlanner.h) add_executable(MqttToRosNode src/MqttToRosNode.cpp ${PROTO_SRCS} ${PROTO_HDRS}) add_executable(MqttRosConnectionTestNode src/MqttRosConnectionTestNode.cpp ${PROTO_SRCS} ${PROTO_HDRS}) add_executable(${PROJECT_NAME}_safety_zone_spawner src/safety_zone_spawner.cpp) diff --git a/config/config.yaml b/config/config.yaml index 8f88ed54dc19e176d0d8735a031dd0d6d0e0ef43..765962ee832c2b8d4821cf2f9d8d9f22b8056131 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -6,6 +6,7 @@ panda_mqtt_connector: topics: robotConfig: "robotconfig" dataConfig: "dataconfig" + trajectoryConfig: "trajectoryconfig" zone_size: 0.5 zones: - "1 1" diff --git a/src/MqttRosConnectionTestNode.cpp b/src/MqttRosConnectionTestNode.cpp index 0fb917623b671b771de1e80a27caef3bccdbaeae..321af1ec4b27c41a92a8f38aef8aa7c738911028 100644 --- a/src/MqttRosConnectionTestNode.cpp +++ b/src/MqttRosConnectionTestNode.cpp @@ -10,23 +10,11 @@ #include "util/MqttUtil.h" -#include <mqtt/client.h> - -using namespace std; - -/* - * mqtt-topics for all links - * ground, link0, link1, link2, link3, link4, link5, link6, link7 (end-effector), left finger, right finder - */ -std::string topics[11] = {"panda_ground", "panda_link_0", "panda_link_1", "panda_link_2", - "panda_link_3", "panda_link_4", "panda_link_5", "panda_link_6", - "panda_link_7", "panda_link_8", "panda_link_9"}; - -const string CLIENT_ID{"ros_mqtt_tester"}; +const std::string CLIENT_ID{"ros_mqtt_tester"}; MqttUtil *mqttUtil = nullptr; -void testTrajectoryUpdate(ros::NodeHandle n) { +void testTrajectoryUpdate(const ros::NodeHandle& n) { ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", ">>>>>>> STARTING TRAJECTORY UPDATE TEST <<<<<<<"); @@ -52,7 +40,6 @@ void testTrajectoryUpdate(ros::NodeHandle n) { auto pubmsg = mqtt::make_message("trajectoryconfig", mqtt_msg); mqttUtil->getClient().publish(pubmsg); - ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", ">>>>>>>>>>>> CHECKING SERVER <<<<<<<<<<<<<"); std::vector<double> x_pos; std::vector<double> y_pos; @@ -60,25 +47,85 @@ void testTrajectoryUpdate(ros::NodeHandle n) { ros::Duration(5.0).sleep(); - ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", "x configured: " << n.getParam("trajectory_pos_x", x_pos)); - ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", "y configured: " << n.getParam("trajectory_pos_y", y_pos)); - ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", "z configured: " << n.getParam("trajectory_pos_z", z_pos)); + ROS_INFO_STREAM("x configured: " << n.getParam("trajectory_pos_x", x_pos)); + ROS_INFO_STREAM("y configured: " << n.getParam("trajectory_pos_y", y_pos)); + ROS_INFO_STREAM("z configured: " << n.getParam("trajectory_pos_z", z_pos)); for (int i = 0; i < x_pos.size(); i++) { - ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", "X POS: " << x_pos.at(i)); - ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", "Y POS: " << y_pos.at(i)); - ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", "Z POS: " << z_pos.at(i)); + ROS_INFO_STREAM("X POS: " << x_pos.at(i)); + ROS_INFO_STREAM("Y POS: " << y_pos.at(i)); + ROS_INFO_STREAM("Z POS: " << z_pos.at(i)); } } -void testPlanningModeChange(ros::NodeHandle n) { +void testPlanningModeChange(const ros::NodeHandle &n) { + + std::string planning_mode; + if (!n.getParam("robot_planning_mode", planning_mode)) { + ROS_ERROR_STREAM("No " << n.getNamespace() << "/robot_planning_mode set. Aborting planning mode change test."); + return; + } - ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", ">>>>>>> STARTING PLANNING MODE CHANGE TEST <<<<<<<"); + double motionSpeedFactor; + if (!n.getParam("robot_speed_factor", motionSpeedFactor)) { + ROS_ERROR_STREAM("No " << n.getNamespace() << "/robot_speed_factor set. Aborting planning mode change test."); + return; + } config::RobotConfig rc; - rc.set_speed(1.0); - rc.set_planningmode(config::RobotConfig_PlanningMode_FLUID); + rc.set_speed(motionSpeedFactor); + + if (planning_mode == "fluid_path") { + rc.set_planningmode(config::RobotConfig_PlanningMode_CARTESIAN); + ROS_INFO_STREAM("PLANNING MODE CHANGE TEST: changed from fluid_path to cartesian_path"); + } else { + rc.set_planningmode(config::RobotConfig_PlanningMode_FLUID); + ROS_INFO_STREAM("PLANNING MODE CHANGE TEST: changed from cartesian_path to fluid_path"); + } + + rc.set_looptrajectory(true); + + std::string mqtt_msg; + rc.SerializeToString(&mqtt_msg); + + auto pubmsg = mqtt::make_message("robotconfig", mqtt_msg); + mqttUtil->getClient().publish(pubmsg); +} + +void testSpeedFactorChange(const ros::NodeHandle &n) { + + std::string planning_mode; + if (!n.getParam("robot_planning_mode", planning_mode)) { + ROS_ERROR_STREAM("No " << n.getNamespace() << "/robot_planning_mode set. Aborting planning mode change test."); + return; + } + + config::RobotConfig rc; + + if (planning_mode == "cartesian_path") { + rc.set_planningmode(config::RobotConfig_PlanningMode_CARTESIAN); + } else { + rc.set_planningmode(config::RobotConfig_PlanningMode_FLUID); + } + + double motionSpeedFactor; + if (!n.getParam("robot_speed_factor", motionSpeedFactor)) { + ROS_ERROR_STREAM("No " << n.getNamespace() << "/robot_speed_factor set. Aborting planning mode change test."); + return; + } + + if (motionSpeedFactor < 0.5) { + rc.set_speed(motionSpeedFactor); + ROS_INFO_STREAM("SPEED FACTOR CHANGE TEST: changed from " << motionSpeedFactor << " to 0.9"); + rc.set_speed(0.9); + } else { + rc.set_planningmode(config::RobotConfig_PlanningMode_FLUID); + ROS_INFO_STREAM("SPEED FACTOR CHANGE TEST: changed from " << motionSpeedFactor << " to 0.2"); + rc.set_speed(0.2); + } + + rc.set_looptrajectory(true); std::string mqtt_msg; rc.SerializeToString(&mqtt_msg); @@ -87,9 +134,9 @@ void testPlanningModeChange(ros::NodeHandle n) { mqttUtil->getClient().publish(pubmsg); } -void testConfig(ros::NodeHandle n) { +void testConfig(const ros::NodeHandle &n) { - ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", ">>>>>>> STARTING CONFIG UPDATE TEST <<<<<<<"); + ROS_INFO_STREAM("DATA CONFIG TEST: publishing all data at a rate of 1000"); config::DataConfig dc; @@ -106,15 +153,18 @@ void testConfig(ros::NodeHandle n) { mqttUtil->getClient().publish(pubmsg); } -void receiveMqttMessages(ros::NodeHandle n) { +void receiveMqttMessages(const ros::NodeHandle &n) { if (mqttUtil->ensureConnection()) { - mqtt::const_message_ptr *msg = new mqtt::const_message_ptr; - if (mqttUtil->getClient().try_consume_message_for(msg, std::chrono::milliseconds (500))) { + auto *msg = new mqtt::const_message_ptr; + if (mqttUtil->getClient().try_consume_message_for(msg, std::chrono::milliseconds(500))) { + ROS_INFO_STREAM("Received message on topic " << msg->get()->get_topic()); if (msg->get()->get_topic() == "test_config") { testConfig(n); } else if (msg->get()->get_topic() == "test_mode_change") { testPlanningModeChange(n); + } else if (msg->get()->get_topic() == "test_speed_change") { + testSpeedFactorChange(n); } else if (msg->get()->get_topic() == "test_trajectory_update") { testTrajectoryUpdate(n); } @@ -141,6 +191,7 @@ int main(int argc, char **argv) { mqttUtil->addTopic("test_config"); mqttUtil->addTopic("test_mode_change"); + mqttUtil->addTopic("test_speed_change"); mqttUtil->addTopic("test_trajectory_update"); mqttUtil->connect(); diff --git a/src/MqttToRosNode.cpp b/src/MqttToRosNode.cpp index 0dda8ccb2c39b70d09f5dc472347b3c7cdf7f250..04a5be7cf9e531023119a62c89b0a261ae4c8ec9 100644 --- a/src/MqttToRosNode.cpp +++ b/src/MqttToRosNode.cpp @@ -1,10 +1,7 @@ #include "mqtt/client.h" -#include <chrono> -#include <iostream> -#include <string> -#include <thread> #include "ros/ros.h" +#include "geometry_msgs/PoseArray.h" #include "util/MqttUtil.h" @@ -12,63 +9,59 @@ #include "robotconfig.pb.h" #include "trajectory.pb.h" -using namespace std; -using namespace std::chrono; +const std::string CLIENT_ID{"mqtt_to_ros"}; -const string CLIENT_ID{"mqtt_to_ros"}; +const std::string ROBOT_CONFIG{"robotconfig"}; +const std::string DATA_CONFIG{"dataconfig"}; +const std::string TRAJECTORY_CONFIG{"trajectoryconfig"}; MqttUtil *mqttUtil = nullptr; -void handleRobotConfig(config::RobotConfig robotConfig, ros::NodeHandle n) { - std::cout << "Retrieved new target-speed: " << robotConfig.speed() << std::endl; +void handleRobotConfig(const config::RobotConfig &robotConfig, const ros::NodeHandle &n) { + ROS_INFO_STREAM("Retrieved new target-speed: " << robotConfig.speed()); n.setParam("robot_speed_factor", robotConfig.speed()); - std::cout << "Retrieved new loop-mode: " << robotConfig.looptrajectory() << std::endl; + ROS_INFO_STREAM("Retrieved new loop-mode: " << robotConfig.looptrajectory()); n.setParam("loop_trajectory", robotConfig.looptrajectory()); std::cout << "Retrieved new planning-mode: " << std::endl; switch (robotConfig.planningmode()) { - case config::RobotConfig_PlanningMode_FLUID: - n.setParam("robot_planning_mode", "fluid_path"); - std::cout << "Planning-mode: fluid" << std::endl; - break; - case config::RobotConfig_PlanningMode_CARTESIAN: - n.setParam("robot_planning_mode", "cartesian_path"); - std::cout << "Planning-mode: cartesian" << std::endl; - break; + case config::RobotConfig_PlanningMode_FLUID: + n.setParam("robot_planning_mode", "fluid_path"); + ROS_INFO_STREAM("Planning-mode: fluid"); + break; + case config::RobotConfig_PlanningMode_CARTESIAN: + n.setParam("robot_planning_mode", "cartesian_path"); + ROS_INFO_STREAM("Planning-mode: cartesian"); + break; } } -void handleNewTrajectory(plan::Trajectory trajectory, ros::NodeHandle n) { - ROS_INFO("Received new trajectory"); +void +handleNewTrajectory(const plan::Trajectory &trajectory, const ros::NodeHandle &n, const ros::Publisher &posePublisher) { - std::vector<double> x_values; - std::vector<double> y_values; - std::vector<double> z_values; + geometry_msgs::PoseArray poseArray; - for (int i = 0; i < trajectory.pos().size(); i++) { - x_values.push_back(trajectory.pos().Get(i).x()); - y_values.push_back(trajectory.pos().Get(i).y()); - z_values.push_back(trajectory.pos().Get(i).z()); + for (const auto &protoPose : trajectory.pos()) { + geometry_msgs::Pose pose; + pose.orientation.w = 1; + pose.position.x = protoPose.x(); + pose.position.y = protoPose.y(); + pose.position.z = protoPose.z(); + poseArray.poses.push_back(pose); } - n.deleteParam("trajectory_pos_x"); - n.deleteParam("trajectory_pos_y"); - n.deleteParam("trajectory_pos_z"); + posePublisher.publish(poseArray); - n.setParam("trajectory_pos_x", x_values); - n.setParam("trajectory_pos_y", y_values); - n.setParam("trajectory_pos_z", z_values); - - n.setParam("new_trajectory_available", true); } -void handleDataConfig(config::DataConfig dataConfig, ros::NodeHandle n) { - std::cout << "Retrieved new data-config: -- publish-rate: " << dataConfig.publishrate() - << " -- enablePosition: " << dataConfig.enableposition() - << " -- enableOrientation: " << dataConfig.enableorientation() - << " -- enableTwistLinear: " << dataConfig.enabletwistlinear() - << " -- enableTwistAngular: " << dataConfig.enabletwistangular() << std::endl; +void handleDataConfig(const config::DataConfig &dataConfig, const ros::NodeHandle &n) { + ROS_INFO_STREAM("Retrieved new data-config:" << std::endl + << " -- publish-rate: " << dataConfig.publishrate() << std::endl + << " -- enablePosition: " << dataConfig.enableposition() << std::endl + << " -- enableOrientation: " << dataConfig.enableorientation() << std::endl + << " -- enableTwistLinear: " << dataConfig.enabletwistlinear() << std::endl + << " -- enableTwistAngular: " << dataConfig.enabletwistangular() << std::endl); n.setParam("data_publish_rate", dataConfig.publishrate()); n.setParam("data_enable_position", dataConfig.enableposition()); @@ -77,36 +70,27 @@ void handleDataConfig(config::DataConfig dataConfig, ros::NodeHandle n) { n.setParam("data_enable_twist_angular", dataConfig.enabletwistangular()); } -void handleMessage(const ros::NodeHandle &n, const mqtt::const_message_ptr &msg) { - - if (msg->get_topic() == "robotconfig") { - const string rc_payload = msg->get_payload_str(); - config::RobotConfig robotConfig; - robotConfig.ParseFromString(rc_payload); - handleRobotConfig(robotConfig, n); - } - - if (msg->get_topic() == "dataconfig") { - const string dc_payload = msg->get_payload_str(); - config::DataConfig dataConfig; - dataConfig.ParseFromString(dc_payload); - handleDataConfig(dataConfig, n); - } - - if (msg->get_topic() == "trajectoryconfig") { - const string tc_payload = msg->get_payload_str(); - plan::Trajectory trajectoryConfig; - trajectoryConfig.ParseFromString(tc_payload); - handleNewTrajectory(trajectoryConfig, n); - } -} - -void receiveMqttMessages(ros::NodeHandle n) { +void receiveMqttMessages(const ros::NodeHandle &n, const ros::Publisher &posePublisher) { if (mqttUtil->ensureConnection()) { mqtt::const_message_ptr msg; if (mqttUtil->getClient().try_consume_message_for(&msg, std::chrono::milliseconds(500))) { ROS_INFO_STREAM("NEW MESSAGE ON TOPIC " << msg.get()->get_topic()); - handleMessage(n, msg); + if (msg->get_topic() == ROBOT_CONFIG) { + const std::string rc_payload = msg->get_payload_str(); + config::RobotConfig robotConfig; + robotConfig.ParseFromString(rc_payload); + handleRobotConfig(robotConfig, n); + } else if (msg->get_topic() == DATA_CONFIG) { + const std::string dc_payload = msg->get_payload_str(); + config::DataConfig dataConfig; + dataConfig.ParseFromString(dc_payload); + handleDataConfig(dataConfig, n); + } else if (msg->get_topic() == TRAJECTORY_CONFIG) { + const std::string tc_payload = msg->get_payload_str(); + plan::Trajectory trajectoryConfig; + trajectoryConfig.ParseFromString(tc_payload); + handleNewTrajectory(trajectoryConfig, n, posePublisher); + } } } else { ROS_ERROR_STREAM_NAMED("MqttToRosNode", "Not connected! Unable to listen to messages."); @@ -117,17 +101,27 @@ int main(int argc, char *argv[]) { ros::init(argc, argv, "mqtt_listener"); ros::NodeHandle n("panda_mqtt_connector"); + ros::Publisher posePublisher = n.advertise<geometry_msgs::PoseArray>("poses", 1000); + std::string server; if (!n.getParam("server", server)) { - ROS_ERROR("Could not get string value for panda_mqtt_connector/server from param server"); + ROS_ERROR_STREAM("Could not get string value for " << n.getNamespace() << "/server from param server"); return -1; } mqttUtil = new MqttUtil(CLIENT_ID, server); - mqttUtil->addTopic("robotconfig"); - mqttUtil->addTopic("dataconfig"); - mqttUtil->addTopic("trajectoryconfig"); + std::string robotConfig = "robotconfig"; + + std::map<std::string, std::string> topics; + if (!n.getParam("topics", topics)) { + ROS_ERROR_STREAM("Could not get string value for " << n.getNamespace() << "/topics from param server"); + } + + for (const auto &topic : topics) { + ROS_INFO_STREAM("Listening to MQTT topic " << topic.second); + mqttUtil->addTopic(topic.second); + } if (!mqttUtil->connect()) { ROS_ERROR_STREAM("Unable to connect to MQTT server. Exiting..."); @@ -135,7 +129,7 @@ int main(int argc, char *argv[]) { } while (ros::ok()) { - receiveMqttMessages(n); + receiveMqttMessages(n, posePublisher); ros::spinOnce(); } diff --git a/src/TimedPlanner.cpp b/src/TimedPlanner.cpp index cc19a23ed8dc67600d1030988f07c940d9a079d6..ecd93e5a32a504e52b95eecb054762011203fef4 100644 --- a/src/TimedPlanner.cpp +++ b/src/TimedPlanner.cpp @@ -10,24 +10,17 @@ #include <moveit/trajectory_processing/iterative_time_parameterization.h> #include <tf2/transform_datatypes.h> #include <tf2_geometry_msgs/tf2_geometry_msgs.h> +#include "geometry_msgs/PoseArray.h" + +#include <boost/optional.hpp> #include "util/TrajectoryUtil.h" +#include "TimedPlanner.h" #include <cmath> +#include <utility> -namespace PlannerState { - - std::vector<geometry_msgs::Pose> raw_trajectory; - TrajectoryUtil traj_util; - - const double default_velocity = 1.0; - const std::string default_planning_mode = TrajectoryUtil::FLUID_PATH; - - bool isLooping = true; - -} - void constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects, double x_dimension, double y_dimension) { @@ -53,8 +46,9 @@ constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects, dou collision_objects.push_back(plate); } -void loadCircularTrajectory(std::vector<geometry_msgs::Pose> &waypoints, double radius, geometry_msgs::Point origin, - int granularity) { +void TimedPlanner::loadCircularTrajectory(double radius, geometry_msgs::Point origin, int granularity) { + + nextPoses = geometry_msgs::PoseArray{}; double step_size_rad = 2 * M_PI / granularity; @@ -73,39 +67,23 @@ void loadCircularTrajectory(std::vector<geometry_msgs::Pose> &waypoints, double nextPose.position.y = origin.y + radius * cos(next_rad); nextPose.position.z = origin.z; - waypoints.push_back(nextPose); + nextPoses.get().poses.push_back(nextPose); } } -bool loadWaypoints(std::vector<geometry_msgs::Pose> &waypoints, const ros::NodeHandle &node_handle, - geometry_msgs::Pose &initialPose) { - - // regardless if the loading fails, the trajectory is no longer "new" after calling this method - node_handle.setParam("new_trajectory_available", false); - - std::vector<double> x_values; - std::vector<double> y_values; - std::vector<double> z_values; - - // check if no trajectory is configured - if (!node_handle.getParam("trajectory_pos_x", x_values) || - !node_handle.getParam("trajectory_pos_y", y_values) || - !node_handle.getParam("trajectory_pos_z", z_values)) { - return false; - } else { - for (int i = 0; i < x_values.size(); i++) { - geometry_msgs::Pose pose = initialPose; - pose.position.x = x_values.at(i); - pose.position.y = y_values.at(i); - pose.position.z = z_values.at(i); - waypoints.push_back(pose); - } +bool TimedPlanner::loadWaypoints() { + if (nextPoses.is_initialized()) { + poses = nextPoses.get(); + nextPoses.reset(); return true; + } else { + return false; } } -static std::vector<moveit_msgs::RobotTrajectory> split(moveit::planning_interface::MoveGroupInterface &group, const moveit::planning_interface::MoveGroupInterface::Plan &plan) { +std::vector<moveit_msgs::RobotTrajectory> TimedPlanner::split(moveit::planning_interface::MoveGroupInterface &group, + const moveit::planning_interface::MoveGroupInterface::Plan &plan) { std::vector<moveit_msgs::RobotTrajectory> trajectories; @@ -119,7 +97,7 @@ static std::vector<moveit_msgs::RobotTrajectory> split(moveit::planning_interfac robot_trajectory::RobotTrajectory wpTrajectory{robotModel, "panda_arm"}; moveit::core::RobotState waypointState{robotModel}; wpTrajectory.insertWayPoint(0, rt.getWayPoint(wpIndex), 0); - wpTrajectory.insertWayPoint(1, rt.getWayPoint(wpIndex+1), rt.getWayPointDurationFromPrevious(wpIndex+1)); + wpTrajectory.insertWayPoint(1, rt.getWayPoint(wpIndex + 1), rt.getWayPointDurationFromPrevious(wpIndex + 1)); moveit_msgs::RobotTrajectory wpTrajectoryMsg{}; wpTrajectory.getRobotTrajectoryMsg(wpTrajectoryMsg); trajectories.push_back(wpTrajectoryMsg); @@ -129,44 +107,34 @@ static std::vector<moveit_msgs::RobotTrajectory> split(moveit::planning_interfac } -void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::MoveGroupInterface &group) { - - bool newTrajectoryAvailable = false; - node_handle.getParam("new_trajectory_available", newTrajectoryAvailable); +void TimedPlanner::doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::MoveGroupInterface &group) { - if (newTrajectoryAvailable) { - geometry_msgs::Pose current_pose = group.getCurrentPose().pose; - PlannerState::raw_trajectory.clear(); + loadWaypoints(); - if (!loadWaypoints(PlannerState::raw_trajectory, node_handle, current_pose)) { - ROS_ERROR_STREAM("Unable to load new trajectory!"); - } - } - - if (PlannerState::raw_trajectory.empty()) { - return; - } - - for (int i = 0; i < PlannerState::raw_trajectory.size(); i++) { + for (int i = 0; i < poses.poses.size(); i++) { - node_handle.getParam("new_trajectory_available", newTrajectoryAvailable); - node_handle.getParam("loop_trajectory", PlannerState::isLooping); + node_handle.getParam("loop_trajectory", isLooping); std::string planning_mode; if (!node_handle.getParam("robot_planning_mode", planning_mode)) { - planning_mode = PlannerState::default_planning_mode; + planning_mode = default_planning_mode; } ROS_INFO_STREAM("Planning the next trajectory in " << planning_mode); moveit::planning_interface::MoveGroupInterface::Plan plan; - if (PlannerState::traj_util.computePathToPose(group, plan, PlannerState::raw_trajectory.at(i), planning_mode, 0.8, - 0.8)) { + if (traj_util.computePathToPose(group, plan, poses.poses.at(i), planning_mode, 0.8, + 0.8)) { + + if (nextPoses.is_initialized()) { + return; + } + for (auto trajectory : split(group, plan)) { double motionSpeedFactor; if (!node_handle.getParam("robot_speed_factor", motionSpeedFactor)) { - motionSpeedFactor = PlannerState::default_velocity; + motionSpeedFactor = default_velocity; } if (motionSpeedFactor <= 0 || motionSpeedFactor > 1.0) { ROS_ERROR_STREAM("Invalid motion speed factor " << motionSpeedFactor << ". Must be in (0,1]."); @@ -184,71 +152,93 @@ void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::Mo } // make sure the robot moves in an infinite circle - if (i == (PlannerState::raw_trajectory.size() - 1)) { - if (PlannerState::isLooping) { + if (i == (poses.poses.size() - 1)) { + if (isLooping) { i = -1; } } } } -int main(int argc, char **argv) { +void TimedPlanner::newPoseCallback(const geometry_msgs::PoseArray::ConstPtr &msg) { + ROS_INFO_STREAM("Received new pose list with " << msg->poses.size() << " poses."); + nextPoses = *msg; +} - // setup this ros-node - ros::init(argc, argv, "timed_cartesian_planner"); - ros::NodeHandle node_handle("panda_mqtt_connector"); - ros::AsyncSpinner spinner(1); - spinner.start(); +TimedPlanner::TimedPlanner(moveit::planning_interface::MoveGroupInterface &group, const double defaultVelocity, + std::string defaultPlanningMode) : default_velocity( + defaultVelocity), default_planning_mode(std::move(defaultPlanningMode)) { - // wait for robot init of robot_state_initializer - ros::Duration(3.0).sleep(); - ROS_INFO(">>>>>>>>>>>>>>>>> START UP FINISHED <<<<<<<<<<<<<<<< "); - - // Initialize start state of robot and target trajectory. - moveit::planning_interface::MoveGroupInterface group("panda_arm"); - moveit::planning_interface::PlanningSceneInterface planning_scene_interface; + ros::NodeHandle node_handle("panda_mqtt_connector"); - node_handle.getParam("loop_trajectory", PlannerState::isLooping); + node_handle.getParam("loop_trajectory", isLooping); std::string defaultTrajectory = "<none>"; node_handle.getParam("default_trajectory", defaultTrajectory); - geometry_msgs::Pose base_pose = group.getCurrentPose().pose; - if (loadWaypoints(PlannerState::raw_trajectory, node_handle, base_pose)) { - ROS_INFO_STREAM("Ignoring default waypoints because there is a trajectory defined by parameters"); + if (loadWaypoints()) { + ROS_INFO_STREAM("Ignoring default waypoints because poses were already received as a message"); } else if (defaultTrajectory == "circle") { ROS_INFO_STREAM("loading default trajectory 'circle'"); geometry_msgs::Point origin; origin.z = .3; - loadCircularTrajectory(PlannerState::raw_trajectory, 0.6, origin, 20); + loadCircularTrajectory(0.6, origin, 20); } else if (defaultTrajectory == "square") { - ROS_INFO_STREAM("loading default trajectory 'square'"); - // choose a default trajectory - base_pose.position.x = 0.4; - base_pose.position.y = 0.4; - base_pose.position.z = 0.3; - geometry_msgs::Pose target_pose_1 = base_pose; - geometry_msgs::Pose target_pose_2 = base_pose; - geometry_msgs::Pose target_pose_3 = base_pose; - geometry_msgs::Pose target_pose_4 = base_pose; - - PlannerState::raw_trajectory.push_back(target_pose_1); - - target_pose_2.position.y *= -1; - PlannerState::raw_trajectory.push_back(target_pose_2); - - target_pose_3.position.x *= -1; - target_pose_3.position.y *= -1; - PlannerState::raw_trajectory.push_back(target_pose_3); - - target_pose_4.position.x *= -1; - PlannerState::raw_trajectory.push_back(target_pose_4); + loadSquareTrajectory(base_pose); + } else { ROS_WARN_STREAM("not loading a default trajectory! provided trajectory: " + defaultTrajectory); } +} + +void TimedPlanner::loadSquareTrajectory(geometry_msgs::Pose base_pose) { + ROS_INFO_STREAM("loading default trajectory 'square'"); + // choose a default trajectory + base_pose.position.x = 0.4; + base_pose.position.y = 0.4; + base_pose.position.z = 0.3; + geometry_msgs::Pose target_pose_1 = base_pose; + geometry_msgs::Pose target_pose_2 = base_pose; + geometry_msgs::Pose target_pose_3 = base_pose; + geometry_msgs::Pose target_pose_4 = base_pose; + + poses.poses.push_back(target_pose_1); + + target_pose_2.position.y *= -1; + poses.poses.push_back(target_pose_2); + + target_pose_3.position.x *= -1; + target_pose_3.position.y *= -1; + poses.poses.push_back(target_pose_3); + + target_pose_4.position.x *= -1; + poses.poses.push_back(target_pose_4); +} + + +int main(int argc, char **argv) { + + // setup this ros-node + ros::init(argc, argv, "timed_planner"); + ros::NodeHandle node_handle("panda_mqtt_connector"); + ros::AsyncSpinner spinner(1); + spinner.start(); + + // wait for robot init of robot_state_initializer + ros::Duration(3.0).sleep(); + ROS_INFO(">>>>>>>>>>>>>>>>> START UP FINISHED <<<<<<<<<<<<<<<< "); + + // Initialize start state of robot and target trajectory. + moveit::planning_interface::MoveGroupInterface group("panda_arm"); + moveit::planning_interface::PlanningSceneInterface planning_scene_interface; + + TimedPlanner planner(group, .5, TrajectoryUtil::FLUID_PATH); + + ros::Subscriber sub = node_handle.subscribe("poses", 1000, &TimedPlanner::newPoseCallback, &planner); + // add the ground plate std::vector<moveit_msgs::CollisionObject> collision_objects; constructPlate(collision_objects, 5.0, 5.0); @@ -257,7 +247,7 @@ int main(int argc, char **argv) { while (ros::ok()) { // execute a trajectory - doMotion(node_handle, group); + planner.doMotion(node_handle, group); ros::spinOnce(); } diff --git a/src/TimedPlanner.h b/src/TimedPlanner.h new file mode 100644 index 0000000000000000000000000000000000000000..ed0b03ada166525bae6f5a8f659b916b57a21e8e --- /dev/null +++ b/src/TimedPlanner.h @@ -0,0 +1,43 @@ +// +// Created by Johannes Mey on 13.07.20. +// + +#ifndef SRC_TIMEDPLANNER_H +#define SRC_TIMEDPLANNER_H + + +#include <geometry_msgs/PoseArray.h> +#include <boost/optional.hpp> +#include "util/TrajectoryUtil.h" + +class TimedPlanner { + +public: + const double default_velocity = 1.0; + const std::string default_planning_mode = TrajectoryUtil::FLUID_PATH; + + static std::vector<moveit_msgs::RobotTrajectory> split(moveit::planning_interface::MoveGroupInterface &group, + const moveit::planning_interface::MoveGroupInterface::Plan &plan); + + void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::MoveGroupInterface &group); + + TimedPlanner(moveit::planning_interface::MoveGroupInterface &group, double defaultVelocity, + std::string defaultPlanningMode); + + void newPoseCallback(const geometry_msgs::PoseArray::ConstPtr &msg); + +private: + geometry_msgs::PoseArray poses; + boost::optional<geometry_msgs::PoseArray> nextPoses; + TrajectoryUtil traj_util; + bool isLooping = true; + + void loadCircularTrajectory(double radius, geometry_msgs::Point origin, int granularity); + + bool loadWaypoints(); + + void loadSquareTrajectory(geometry_msgs::Pose base_pose); +}; + + +#endif //SRC_TIMEDPLANNER_H