From c54909286feb1856d326a8a98d854a6f82b8c0d0 Mon Sep 17 00:00:00 2001 From: Johannes Mey <johannes.mey@tu-dresden.de> Date: Wed, 15 Jul 2020 15:02:06 +0200 Subject: [PATCH] improve trajectory test --- src/MqttRosConnectionTestNode.cpp | 39 ++++++++++++++++++++----------- 1 file changed, 26 insertions(+), 13 deletions(-) diff --git a/src/MqttRosConnectionTestNode.cpp b/src/MqttRosConnectionTestNode.cpp index 7df8993..8bfa5ac 100644 --- a/src/MqttRosConnectionTestNode.cpp +++ b/src/MqttRosConnectionTestNode.cpp @@ -1,26 +1,32 @@ // // Created by sebastian on 31.03.20. // +#include <sstream> + #include "config.pb.h" #include "trajectory.pb.h" #include "ros/ros.h" #include "util/MqttUtil.h" +#include "util/TrajectoryUtil.h" const std::string CLIENT_ID{"ros_mqtt_tester"}; MqttUtil *mqttUtil = nullptr; -void testTrajectoryUpdate(const ros::NodeHandle& n) { +void testTrajectoryUpdate(bool loop, plan::Trajectory_PlanningMode mode) { - ROS_INFO_STREAM("TRAJECTORY UPDATE TEST: Sending simple two-point trajectory to planner."); + ROS_INFO_STREAM("TRAJECTORY UPDATE TEST: Sending simple two-point trajectory to planner. " + << (loop ? "" : "non-") << "looping, " + << (mode == plan::Trajectory_PlanningMode::Trajectory_PlanningMode_CARTESIAN + ? TrajectoryUtil::CARTESIAN_PATH : TrajectoryUtil::FLUID_PATH)); plan::Trajectory trajectory{}; - trajectory.set_loop(false); + trajectory.set_loop(loop); - plan::Trajectory_Pose* pose = trajectory.add_pose(); - pose->set_mode(plan::Trajectory_PlanningMode_FLUID); + plan::Trajectory_Pose *pose = trajectory.add_pose(); + pose->set_mode(mode); pose->mutable_orientation()->set_w(0); pose->mutable_orientation()->set_x(0.7071067811865476); pose->mutable_orientation()->set_y(0.7071067811865476); @@ -30,7 +36,7 @@ void testTrajectoryUpdate(const ros::NodeHandle& n) { pose->mutable_position()->set_y(0.4); pose->mutable_position()->set_z(0.4); - plan::Trajectory_Pose* secondPose = trajectory.add_pose(); + plan::Trajectory_Pose *secondPose = trajectory.add_pose(); secondPose->CopyFrom(*pose); secondPose->mutable_position()->set_x(-0.4); @@ -42,12 +48,6 @@ void testTrajectoryUpdate(const ros::NodeHandle& n) { 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; double motionSpeedFactor; @@ -80,7 +80,20 @@ void receiveMqttMessages(const ros::NodeHandle &n) { if (msg->get()->get_topic() == "test_speed_change") { testSpeedFactorChange(n); } else if (msg->get()->get_topic() == "test_trajectory_update") { - testTrajectoryUpdate(n); + std::istringstream buffer(msg->get()->get_payload_str()); + std::vector<std::string> arguments((std::istream_iterator<std::string>(buffer)),std::istream_iterator<std::string>()); + + bool loop{false}; + plan::Trajectory_PlanningMode mode{plan::Trajectory_PlanningMode::Trajectory_PlanningMode_FLUID}; + for (auto arg : arguments) { + for (auto & c: arg) c = toupper(c); + if (arg == "LOOP") { + loop = true; + } else if (arg == "CARTESIAN") { + mode = plan::Trajectory_PlanningMode::Trajectory_PlanningMode_CARTESIAN; + } + } + testTrajectoryUpdate(loop, mode); } } -- GitLab