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. // Created by sebastian on 31.03.20.
// //
#include <sstream>
#include "config.pb.h" #include "config.pb.h"
#include "trajectory.pb.h" #include "trajectory.pb.h"
#include "ros/ros.h" #include "ros/ros.h"
#include "util/MqttUtil.h" #include "util/MqttUtil.h"
#include "util/TrajectoryUtil.h"
const std::string CLIENT_ID{"ros_mqtt_tester"}; const std::string CLIENT_ID{"ros_mqtt_tester"};
MqttUtil *mqttUtil = nullptr; 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{}; plan::Trajectory trajectory{};
trajectory.set_loop(false); trajectory.set_loop(loop);
plan::Trajectory_Pose* pose = trajectory.add_pose(); 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_w(0);
pose->mutable_orientation()->set_x(0.7071067811865476); pose->mutable_orientation()->set_x(0.7071067811865476);
pose->mutable_orientation()->set_y(0.7071067811865476); pose->mutable_orientation()->set_y(0.7071067811865476);
...@@ -30,7 +36,7 @@ void testTrajectoryUpdate(const ros::NodeHandle& n) { ...@@ -30,7 +36,7 @@ void testTrajectoryUpdate(const ros::NodeHandle& n) {
pose->mutable_position()->set_y(0.4); pose->mutable_position()->set_y(0.4);
pose->mutable_position()->set_z(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->CopyFrom(*pose);
secondPose->mutable_position()->set_x(-0.4); secondPose->mutable_position()->set_x(-0.4);
...@@ -42,12 +48,6 @@ void testTrajectoryUpdate(const ros::NodeHandle& n) { ...@@ -42,12 +48,6 @@ void testTrajectoryUpdate(const ros::NodeHandle& n) {
void testSpeedFactorChange(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; config::RobotConfig rc;
double motionSpeedFactor; double motionSpeedFactor;
...@@ -80,7 +80,20 @@ void receiveMqttMessages(const ros::NodeHandle &n) { ...@@ -80,7 +80,20 @@ void receiveMqttMessages(const ros::NodeHandle &n) {
if (msg->get()->get_topic() == "test_speed_change") { if (msg->get()->get_topic() == "test_speed_change") {
testSpeedFactorChange(n); testSpeedFactorChange(n);
} else if (msg->get()->get_topic() == "test_trajectory_update") { } 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