Skip to content
Snippets Groups Projects
Commit c5490928 authored by Johannes Mey's avatar Johannes Mey
Browse files

improve trajectory test

parent f1f8bf9e
No related branches found
No related tags found
No related merge requests found
//
// 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);
pose->set_mode(mode);
pose->mutable_orientation()->set_w(0);
pose->mutable_orientation()->set_x(0.7071067811865476);
pose->mutable_orientation()->set_y(0.7071067811865476);
......@@ -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);
}
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment