From ee2d6158f8ee1e8ccd02d4814462c226994dff18 Mon Sep 17 00:00:00 2001
From: Johannes Mey <johannes.mey@tu-dresden.de>
Date: Tue, 14 Jul 2020 23:19:15 +0200
Subject: [PATCH] new protobuf definitions
---
CMakeLists.txt | 59 ++++++------
msg/Trajectory.msg | 3 +
msg/Waypoint.msg | 3 +
package.xml | 3 +
proto/config.proto | 12 +++
proto/dataconfig.proto | 13 ---
proto/linkstate.proto | 38 --------
proto/robot_state.proto | 37 ++++++++
proto/robotconfig.proto | 16 ----
proto/trajectory.proto | 21 ++++-
src/MqttRosConnectionTestNode.cpp | 114 +++++------------------
src/MqttToRosNode.cpp | 88 ++++++++----------
src/RobotStateProvider.cpp | 150 ++++++++++--------------------
src/TimedPlanner.cpp | 111 ++++++++++------------
src/TimedPlanner.h | 11 +--
src/util/TrajectoryUtil.cpp | 3 -
src/util/TrajectoryUtil.h | 7 +-
17 files changed, 279 insertions(+), 410 deletions(-)
create mode 100644 msg/Trajectory.msg
create mode 100644 msg/Waypoint.msg
create mode 100644 proto/config.proto
delete mode 100644 proto/dataconfig.proto
delete mode 100644 proto/linkstate.proto
create mode 100644 proto/robot_state.proto
delete mode 100644 proto/robotconfig.proto
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6218192..f8b7f33 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -26,6 +26,8 @@ find_package(catkin REQUIRED COMPONENTS
controller_interface
hardware_interface
simulation_util
+ message_runtime
+ geometry_msgs
)
# System dependencies are found with CMake's conventions
@@ -34,10 +36,7 @@ find_package(Boost REQUIRED COMPONENTS filesystem)
find_package(Protobuf 3 REQUIRED)
include_directories(${Protobuf_INCLUDE_DIRS})
include_directories(${CMAKE_CURRENT_BINARY_DIR})
-#protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/dataconfig.proto)
-#protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/linkstate.proto)
-#protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/robotconfig.proto)
-protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/dataconfig.proto proto/linkstate.proto proto/robotconfig.proto proto/trajectory.proto)
+protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/robot_state.proto proto/config.proto proto/trajectory.proto)
SET_SOURCE_FILES_PROPERTIES(${PROTO_SRC} ${PROTO_HDRS} PROPERTIES GENERATED TRUE)
@@ -52,22 +51,6 @@ set(PAHO_MQTT_C_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/lib/paho.mqtt.c/src")
add_subdirectory(lib/paho.mqtt.cpp)
-# ################################################################################################################################
-# catkin specific configuration ##
-# ################################################################################################################################
-# The catkin_package macro generates cmake config files for your package Declare things to be passed to dependent projects
-catkin_package(CATKIN_DEPENDS
- moveit_core
- moveit_visual_tools
- moveit_ros_planning_interface
- controller_interface
- hardware_interface
- pluginlib
- simulation_util
- DEPENDS
- # system_lib
- )
-
################################################
## Declare ROS messages, services and actions ##
################################################
@@ -93,11 +76,11 @@ catkin_package(CATKIN_DEPENDS
## * 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
-# )
+add_message_files(
+ FILES
+ Trajectory.msg
+ Waypoint.msg
+)
## Generate services in the 'srv' folder
# add_service_files(
@@ -114,10 +97,28 @@ catkin_package(CATKIN_DEPENDS
# )
## Generate added messages and services with any dependencies listed here
-# generate_messages(
-# DEPENDENCIES
-# std_msgs # Or other packages containing msgs
-# )
+generate_messages(
+ DEPENDENCIES
+ geometry_msgs
+ # std_msgs # Or other packages containing msgs
+)
+
+# ################################################################################################################################
+# catkin specific configuration ##
+# ################################################################################################################################
+# The catkin_package macro generates cmake config files for your package Declare things to be passed to dependent projects
+catkin_package(CATKIN_DEPENDS
+ moveit_core
+ moveit_visual_tools
+ moveit_ros_planning_interface
+ controller_interface
+ hardware_interface
+ pluginlib
+ simulation_util
+ geometry_msgs
+ DEPENDS
+ # system_lib
+ )
################################################
## Declare ROS dynamic reconfigure parameters ##
diff --git a/msg/Trajectory.msg b/msg/Trajectory.msg
new file mode 100644
index 0000000..d73ecce
--- /dev/null
+++ b/msg/Trajectory.msg
@@ -0,0 +1,3 @@
+# a (potentially looping) trajectory of Waypoints (a pose and the information of which planner to use to get to it)
+Waypoint[] waypoints # the waypoints thta make up the trajectory
+bool loop # determines if the trajectory should be run in a loop
\ No newline at end of file
diff --git a/msg/Waypoint.msg b/msg/Waypoint.msg
new file mode 100644
index 0000000..1d3153b
--- /dev/null
+++ b/msg/Waypoint.msg
@@ -0,0 +1,3 @@
+# a waypoint in a high-level planning trajectory
+string mode # mode in which the point should be moved to
+geometry_msgs/Pose pose # desired pose in world frame
\ No newline at end of file
diff --git a/package.xml b/package.xml
index d9b8161..80cdcef 100644
--- a/package.xml
+++ b/package.xml
@@ -43,6 +43,9 @@
<depend>controller_interface</depend>
<depend>hardware_interface</depend>
<depend>simulation_util</depend>
+ <build_depend>message_generation</build_depend>
+ <exec_depend>message_runtime</exec_depend>
+ <depend>geometry_msgs</depend>
<exec_depend>pluginlib</exec_depend>
diff --git a/proto/config.proto b/proto/config.proto
new file mode 100644
index 0000000..74f231c
--- /dev/null
+++ b/proto/config.proto
@@ -0,0 +1,12 @@
+syntax = "proto3";
+
+package config;
+
+message RobotConfig {
+ double speed = 1;
+}
+
+message DataConfig {
+ repeated string links = 1;
+ double publishRate = 2;
+}
\ No newline at end of file
diff --git a/proto/dataconfig.proto b/proto/dataconfig.proto
deleted file mode 100644
index 3478e61..0000000
--- a/proto/dataconfig.proto
+++ /dev/null
@@ -1,13 +0,0 @@
-syntax = "proto3";
-
-package config;
-
-message DataConfig {
-
- bool enablePosition = 1;
- bool enableOrientation = 2;
- bool enableTwistLinear = 3;
- bool enableTwistAngular = 4;
-
- int32 publishRate = 5;
-}
\ No newline at end of file
diff --git a/proto/linkstate.proto b/proto/linkstate.proto
deleted file mode 100644
index c07d9d8..0000000
--- a/proto/linkstate.proto
+++ /dev/null
@@ -1,38 +0,0 @@
-syntax = "proto3";
-
-package panda;
-
-message PandaLinkState {
-
- string name = 1;
-
- message Position {
- float positionX = 1;
- float positionY = 2;
- float positionZ = 3;
- }
-
- message Orientation {
- float orientationX = 1;
- float orientationY = 2;
- float orientationZ = 3;
- float orientationW = 4;
- }
-
- message TwistLinear {
- float twistLinearX = 1;
- float twistLinearY = 2;
- float twistLinearZ = 3;
- }
-
- message TwistAngular {
- float twistAngularX = 1;
- float twistAngularY = 2;
- float twistAngularZ = 3;
- }
-
- Position pos = 2;
- Orientation orient = 3;
- TwistLinear tl = 4;
- TwistAngular ta = 5;
-}
\ No newline at end of file
diff --git a/proto/robot_state.proto b/proto/robot_state.proto
new file mode 100644
index 0000000..6630631
--- /dev/null
+++ b/proto/robot_state.proto
@@ -0,0 +1,37 @@
+syntax = "proto3";
+
+package robot;
+
+message RobotState {
+
+ message Position {
+ double x = 1;
+ double y = 2;
+ double z = 3;
+ }
+
+ message Orientation {
+ double x = 1;
+ double y = 2;
+ double z = 3;
+ double w = 4;
+ }
+
+ message LinearTwist {
+ double x = 1;
+ double y = 2;
+ double z = 3;
+ }
+
+ message AngularTwist {
+ double x = 1;
+ double y = 2;
+ double z = 3;
+ }
+
+ string name = 1;
+ Position position = 2;
+ Orientation orientation = 3;
+ LinearTwist linear_twist = 4;
+ AngularTwist angular_twist = 5;
+}
\ No newline at end of file
diff --git a/proto/robotconfig.proto b/proto/robotconfig.proto
deleted file mode 100644
index 93750ae..0000000
--- a/proto/robotconfig.proto
+++ /dev/null
@@ -1,16 +0,0 @@
-syntax = "proto3";
-
-package config;
-
-message RobotConfig {
-
- double speed = 1;
- bool loopTrajectory = 2;
-
- enum PlanningMode {
- FLUID = 0;
- CARTESIAN = 1;
- }
-
- PlanningMode planningMode = 3;
-}
\ No newline at end of file
diff --git a/proto/trajectory.proto b/proto/trajectory.proto
index bf0b76f..9886710 100644
--- a/proto/trajectory.proto
+++ b/proto/trajectory.proto
@@ -10,5 +10,24 @@ message Trajectory {
double z = 3;
}
- repeated Position pos = 1;
+ message Orientation {
+ double x = 1;
+ double y = 2;
+ double z = 3;
+ double w = 4;
+ }
+
+ enum PlanningMode {
+ FLUID = 0;
+ CARTESIAN = 1;
+ }
+
+ message Pose {
+ Position position = 1;
+ Orientation orientation = 2;
+ PlanningMode mode = 3;
+ }
+
+ repeated Pose pose = 1;
+ bool loop = 2;
}
\ No newline at end of file
diff --git a/src/MqttRosConnectionTestNode.cpp b/src/MqttRosConnectionTestNode.cpp
index 0bf073e..d4ca978 100644
--- a/src/MqttRosConnectionTestNode.cpp
+++ b/src/MqttRosConnectionTestNode.cpp
@@ -1,12 +1,10 @@
//
// Created by sebastian on 31.03.20.
//
-#include "dataconfig.pb.h"
-#include "robotconfig.pb.h"
+#include "config.pb.h"
#include "trajectory.pb.h"
#include "ros/ros.h"
-#include <gazebo_msgs/LinkStates.h>
#include "util/MqttUtil.h"
@@ -16,81 +14,30 @@ MqttUtil *mqttUtil = nullptr;
void testTrajectoryUpdate(const ros::NodeHandle& n) {
- ROS_INFO_STREAM_NAMED("MqttRosConnectionTestNode", ">>>>>>> STARTING TRAJECTORY UPDATE TEST <<<<<<<");
+ ROS_INFO_STREAM("TRAJECTORY UPDATE TEST: Sending simple two-point trajectory to planner.");
- plan::Trajectory traj;
+ plan::Trajectory trajectory{};
+ trajectory.set_loop(true);
- plan::Trajectory_Position pos1;
- pos1.set_x(0.6);
- pos1.set_y(0.0);
- pos1.set_z(0.6);
+ plan::Trajectory_Pose* pose = trajectory.add_pose();
+ pose->set_mode(plan::Trajectory_PlanningMode_FLUID);
+ pose->mutable_orientation()->set_w(0);
+ pose->mutable_orientation()->set_x(0.7071067811865476);
+ pose->mutable_orientation()->set_y(0.7071067811865476);
+ pose->mutable_orientation()->set_z(0);
- traj.add_pos()->CopyFrom(pos1);
+ pose->mutable_position()->set_x(0.6);
+ pose->mutable_position()->set_y(0.4);
+ pose->mutable_position()->set_z(0.6);
- plan::Trajectory_Position pos2;
- pos2.set_x(-0.6);
- pos2.set_y(0.0);
- pos2.set_z(0.6);
-
- traj.add_pos()->CopyFrom(pos2);
-
- std::string mqtt_msg;
- traj.SerializeToString(&mqtt_msg);
+ plan::Trajectory_Pose* secondPose = trajectory.add_pose();
+ secondPose->CopyFrom(*pose);
+ secondPose->mutable_position()->set_x(-0.6);
+ std::string mqtt_msg{};
+ trajectory.SerializeToString(&mqtt_msg);
auto pubmsg = mqtt::make_message("trajectory", mqtt_msg);
mqttUtil->getClient().publish(pubmsg);
-
-
- std::vector<double> x_pos;
- std::vector<double> y_pos;
- std::vector<double> z_pos;
-
- ros::Duration(5.0).sleep();
-
- 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("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(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;
- }
-
- 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(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) {
@@ -103,12 +50,6 @@ void testSpeedFactorChange(const ros::NodeHandle &n) {
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.");
@@ -116,17 +57,13 @@ void testSpeedFactorChange(const ros::NodeHandle &n) {
}
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);
@@ -134,16 +71,10 @@ void testSpeedFactorChange(const ros::NodeHandle &n) {
mqttUtil->getClient().publish(pubmsg);
}
-void testConfig(const ros::NodeHandle &n) {
-
+void testDataConfig(const ros::NodeHandle &n) {
ROS_INFO_STREAM("DATA CONFIG TEST: publishing all data at a rate of 1000");
config::DataConfig dc;
-
- dc.set_enableposition(true);
- dc.set_enableorientation(true);
- dc.set_enabletwistangular(true);
- dc.set_enabletwistlinear(true);
dc.set_publishrate(1000);
std::string mqtt_msg;
@@ -160,9 +91,7 @@ void receiveMqttMessages(const ros::NodeHandle &n) {
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);
+ testDataConfig(n);
} else if (msg->get()->get_topic() == "test_speed_change") {
testSpeedFactorChange(n);
} else if (msg->get()->get_topic() == "test_trajectory_update") {
@@ -171,7 +100,7 @@ void receiveMqttMessages(const ros::NodeHandle &n) {
}
} else {
- ROS_ERROR_STREAM_NAMED("MqttRosConnectionTestNode", "Not connected! Unable to listen to messages.");
+ ROS_ERROR_STREAM("Not connected! Unable to listen to messages.");
}
}
@@ -190,7 +119,6 @@ int main(int argc, char **argv) {
mqttUtil = new MqttUtil(CLIENT_ID, server);
mqttUtil->addTopic("test_config");
- mqttUtil->addTopic("test_mode_change");
mqttUtil->addTopic("test_speed_change");
mqttUtil->addTopic("test_trajectory_update");
diff --git a/src/MqttToRosNode.cpp b/src/MqttToRosNode.cpp
index da04395..b728ed1 100644
--- a/src/MqttToRosNode.cpp
+++ b/src/MqttToRosNode.cpp
@@ -1,13 +1,15 @@
#include "mqtt/client.h"
#include "ros/ros.h"
-#include "geometry_msgs/PoseArray.h"
#include "util/MqttUtil.h"
-#include "dataconfig.pb.h"
-#include "robotconfig.pb.h"
+#include "panda_mqtt_connector/Trajectory.h"
+#include "panda_mqtt_connector/Waypoint.h"
+
+#include "config.pb.h"
#include "trajectory.pb.h"
+#include "util/TrajectoryUtil.h"
const std::string CLIENT_ID{"mqtt_to_ros"};
@@ -20,60 +22,52 @@ MqttUtil *mqttUtil = nullptr;
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());
-
- ROS_INFO_STREAM("Retrieved new loop-mode: " << robotConfig.looptrajectory());
- n.setParam("loop_trajectory", robotConfig.looptrajectory());
-
- ROS_INFO_STREAM("Retrieved new planning-mode.");
- switch (robotConfig.planningmode()) {
- 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;
- case config::RobotConfig_PlanningMode_RobotConfig_PlanningMode_INT_MIN_SENTINEL_DO_NOT_USE_:
- ROS_ERROR_STREAM("Planning-mode: INT_MIN_SENTINEL_DO_NOT_USE_");
- break;
- case config::RobotConfig_PlanningMode_RobotConfig_PlanningMode_INT_MAX_SENTINEL_DO_NOT_USE_:
- ROS_ERROR_STREAM("Planning-mode: INT_MAX_SENTINEL_DO_NOT_USE_");
- break;
- }
}
-void
-handleNewTrajectory(const plan::Trajectory &trajectory, const ros::NodeHandle &n, const ros::Publisher &posePublisher) {
-
- geometry_msgs::PoseArray poseArray;
+void handleNewTrajectory(const plan::Trajectory &trajectory, const ros::Publisher &posePublisher) {
+
+ panda_mqtt_connector::Trajectory poseArray;
+
+ for (const auto &protoPose : trajectory.pose()) {
+ panda_mqtt_connector::Waypoint waypoint;
+ waypoint.pose.orientation.w = protoPose.orientation().w();
+ waypoint.pose.orientation.x = protoPose.orientation().x();
+ waypoint.pose.orientation.y = protoPose.orientation().y();
+ waypoint.pose.orientation.z = protoPose.orientation().z();
+ waypoint.pose.position.x = protoPose.position().x();
+ waypoint.pose.position.y = protoPose.position().y();
+ waypoint.pose.position.z = protoPose.position().z();
+
+ switch (protoPose.mode()) {
+ case plan::Trajectory_PlanningMode_FLUID:
+ waypoint.mode = TrajectoryUtil::FLUID_PATH;
+ break;
+ case plan::Trajectory_PlanningMode_CARTESIAN:
+ waypoint.mode = TrajectoryUtil::CARTESIAN_PATH;
+ break;
+ default:
+ ROS_WARN("Received pose has invalid mode!");
+ }
- 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);
+ poseArray.waypoints.push_back(waypoint);
}
+ poseArray.loop = trajectory.loop();
+
posePublisher.publish(poseArray);
}
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);
+
+ std::string links;
+ for (const auto &piece : dataConfig.links()) links = links + " " + "links";
+
+ ROS_INFO_STREAM("Retrieved new data-config. links = " << links << "; rate = " << dataConfig.publishrate());
n.setParam("data_publish_rate", dataConfig.publishrate());
- n.setParam("data_enable_position", dataConfig.enableposition());
- n.setParam("data_enable_orientation", dataConfig.enableorientation());
- n.setParam("data_enable_twist_linear", dataConfig.enabletwistlinear());
- n.setParam("data_enable_twist_angular", dataConfig.enabletwistangular());
+
+ // TODO add links to a set
}
void receiveMqttMessages(const ros::NodeHandle &n, const ros::Publisher &posePublisher) {
@@ -95,7 +89,7 @@ void receiveMqttMessages(const ros::NodeHandle &n, const ros::Publisher &posePub
const std::string tc_payload = msg->get_payload_str();
plan::Trajectory trajectoryConfig;
trajectoryConfig.ParseFromString(tc_payload);
- handleNewTrajectory(trajectoryConfig, n, posePublisher);
+ handleNewTrajectory(trajectoryConfig, posePublisher);
}
}
} else {
@@ -107,7 +101,7 @@ 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);
+ ros::Publisher posePublisher = n.advertise<panda_mqtt_connector::Trajectory>("poses", 1000);
std::string server;
if (!n.getParam("server", server)) {
diff --git a/src/RobotStateProvider.cpp b/src/RobotStateProvider.cpp
index 8444434..c31bd08 100644
--- a/src/RobotStateProvider.cpp
+++ b/src/RobotStateProvider.cpp
@@ -9,7 +9,7 @@
#include <gazebo_msgs/LinkStates.h>
#include <mqtt/client.h>
-#include "linkstate.pb.h"
+#include "robot_state.pb.h"
#include "util/MqttUtil.h"
@@ -35,132 +35,82 @@ namespace robot_state_provider {
*/
void exportMessageToLog(const gazebo_msgs::LinkStates &msg) {
- bool export_position = false;
- bool export_orientation = false;
- bool export_twist_linear = false;
- bool export_twist_angular = false;
-
- ros::NodeHandle n;
-
- n.getParam("data_enable_position", export_position);
- n.getParam("data_enable_orientation", export_orientation);
- n.getParam("data_enable_twist_linear", export_twist_linear);
- n.getParam("data_enable_twist_angular", export_twist_angular);
-
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 >>>");
- 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);
- }
+ 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 >>>");
+ 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);
- }
+ 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);
- }
+ 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);
- }
+ 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);
}
}
- std::vector<panda::PandaLinkState> buildLinkStateMessages(const gazebo_msgs::LinkStates &msg) {
-
- bool export_position = false;
- bool export_orientation = false;
- bool export_twist_linear = false;
- bool export_twist_angular = false;
-
- ros::NodeHandle n("panda_mqtt_connector");
-
- if (!n.getParam("data_enable_position", export_position)) {
- export_position = false;
- }
- if (!n.getParam("data_enable_orientation", export_orientation)) {
- export_orientation = false;
- }
- if (!n.getParam("data_enable_twist_linear", export_twist_linear)) {
- export_twist_linear = false;
- }
- if (!n.getParam("data_enable_twist_angular", export_twist_angular)) {
- export_twist_angular = false;
- }
+ std::vector<robot::RobotState> buildLinkStateMessages(const gazebo_msgs::LinkStates &msg) {
- std::vector<panda::PandaLinkState> messages{};
+ std::vector<robot::RobotState> messages{};
std::vector<std::string> names{msg.name};
for (int link_number = 0; link_number < names.size(); ++link_number) {
std::string name = names[link_number];
if (topics.find(name) != topics.end()) {
- panda::PandaLinkState pls_msg;
+ robot::RobotState pls_msg;
// set the name to the MQTT topic provided by the topics map
pls_msg.set_name(topics[name]);
- if (export_position) {
- auto *pos = new panda::PandaLinkState_Position();
- pos->set_positionx(msg.pose.at(link_number).position.x);
- pos->set_positiony(msg.pose.at(link_number).position.y);
- pos->set_positionz(msg.pose.at(link_number).position.z);
- pls_msg.set_allocated_pos(pos);
- }
-
- if (export_orientation) {
- auto *orient = new panda::PandaLinkState_Orientation();
- orient->set_orientationw(msg.pose.at(link_number).orientation.w);
- orient->set_orientationx(msg.pose.at(link_number).orientation.x);
- orient->set_orientationy(msg.pose.at(link_number).orientation.y);
- orient->set_orientationz(msg.pose.at(link_number).orientation.z);
- pls_msg.set_allocated_orient(orient);
- }
-
- if (export_twist_linear) {
- auto *tl = new panda::PandaLinkState_TwistLinear();
- tl->set_twistlinearx(msg.twist.at(link_number).linear.x);
- tl->set_twistlineary(msg.twist.at(link_number).linear.y);
- tl->set_twistlinearz(msg.twist.at(link_number).linear.z);
- pls_msg.set_allocated_tl(tl);
- }
-
- if (export_twist_angular) {
- auto *ta = new panda::PandaLinkState_TwistAngular();
- ta->set_twistangularx(msg.twist.at(link_number).angular.x);
- ta->set_twistangulary(msg.twist.at(link_number).angular.y);
- ta->set_twistangularz(msg.twist.at(link_number).angular.z);
- pls_msg.set_allocated_ta(ta);
- }
+ auto *pos = new robot::RobotState_Position();
+ pos->set_x(msg.pose.at(link_number).position.x);
+ pos->set_y(msg.pose.at(link_number).position.y);
+ pos->set_z(msg.pose.at(link_number).position.z);
+ pls_msg.set_allocated_position(pos);
+
+ auto *orient = new robot::RobotState_Orientation();
+ orient->set_w(msg.pose.at(link_number).orientation.w);
+ orient->set_x(msg.pose.at(link_number).orientation.x);
+ orient->set_y(msg.pose.at(link_number).orientation.y);
+ orient->set_z(msg.pose.at(link_number).orientation.z);
+ pls_msg.set_allocated_orientation(orient);
+
+ auto *tl = new robot::RobotState_LinearTwist();
+ tl->set_x(msg.twist.at(link_number).linear.x);
+ tl->set_y(msg.twist.at(link_number).linear.y);
+ tl->set_z(msg.twist.at(link_number).linear.z);
+ pls_msg.set_allocated_linear_twist(tl);
+
+ auto *ta = new robot::RobotState_AngularTwist();
+ ta->set_x(msg.twist.at(link_number).angular.x);
+ ta->set_y(msg.twist.at(link_number).angular.y);
+ ta->set_z(msg.twist.at(link_number).angular.z);
+ pls_msg.set_allocated_angular_twist(ta);
messages.push_back(pls_msg);
} else {
@@ -175,7 +125,7 @@ namespace robot_state_provider {
if (mqttUtil->ensureConnection()) {
try {
- for (const panda::PandaLinkState& pls_msg : buildLinkStateMessages(msg)) {
+ for (const robot::RobotState &pls_msg : buildLinkStateMessages(msg)) {
std::string mqtt_msg;
if (!pls_msg.SerializeToString(&mqtt_msg)) {
diff --git a/src/TimedPlanner.cpp b/src/TimedPlanner.cpp
index ecd93e5..34b8d3b 100644
--- a/src/TimedPlanner.cpp
+++ b/src/TimedPlanner.cpp
@@ -6,11 +6,13 @@
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/CollisionObject.h>
+#include "panda_mqtt_connector/Trajectory.h"
+#include "panda_mqtt_connector/Waypoint.h"
+
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#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>
@@ -48,7 +50,7 @@ constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects, dou
void TimedPlanner::loadCircularTrajectory(double radius, geometry_msgs::Point origin, int granularity) {
- nextPoses = geometry_msgs::PoseArray{};
+ nextPoses = panda_mqtt_connector::Trajectory{};
double step_size_rad = 2 * M_PI / granularity;
@@ -61,13 +63,13 @@ void TimedPlanner::loadCircularTrajectory(double radius, geometry_msgs::Point or
tf2::Quaternion orientation; // the RPY constructor is deprecated
orientation.setRPY(0, M_PI, next_rad);
- geometry_msgs::Pose nextPose;
- nextPose.orientation = tf2::toMsg(orientation);
- nextPose.position.x = origin.x + radius * sin(next_rad);
- nextPose.position.y = origin.y + radius * cos(next_rad);
- nextPose.position.z = origin.z;
+ panda_mqtt_connector::Waypoint nextPose{};
+ nextPose.pose.orientation = tf2::toMsg(orientation);
+ nextPose.pose.position.x = origin.x + radius * sin(next_rad);
+ nextPose.pose.position.y = origin.y + radius * cos(next_rad);
+ nextPose.pose.position.z = origin.z;
- nextPoses.get().poses.push_back(nextPose);
+ nextPoses->waypoints.push_back(nextPose);
}
}
@@ -111,57 +113,46 @@ void TimedPlanner::doMotion(const ros::NodeHandle &node_handle, moveit::planning
loadWaypoints();
- for (int i = 0; i < poses.poses.size(); i++) {
-
- node_handle.getParam("loop_trajectory", isLooping);
-
- std::string planning_mode;
- if (!node_handle.getParam("robot_planning_mode", planning_mode)) {
- planning_mode = default_planning_mode;
- }
+ while (poses.loop) {
+ for (auto &waypoint : poses.waypoints) {
- ROS_INFO_STREAM("Planning the next trajectory in " << planning_mode);
+ ROS_INFO_STREAM("Planning the next trajectory in " << waypoint.mode);
- moveit::planning_interface::MoveGroupInterface::Plan plan;
+ moveit::planning_interface::MoveGroupInterface::Plan plan;
- if (traj_util.computePathToPose(group, plan, poses.poses.at(i), planning_mode, 0.8,
- 0.8)) {
+ ROS_ERROR_STREAM(waypoint);
- if (nextPoses.is_initialized()) {
- return;
- }
+ if (TrajectoryUtil::computePathToPose(group, plan, waypoint.pose, waypoint.mode, 0.8, 0.8)) {
- for (auto trajectory : split(group, plan)) {
- double motionSpeedFactor;
- if (!node_handle.getParam("robot_speed_factor", motionSpeedFactor)) {
- motionSpeedFactor = default_velocity;
- }
- if (motionSpeedFactor <= 0 || motionSpeedFactor > 1.0) {
- ROS_ERROR_STREAM("Invalid motion speed factor " << motionSpeedFactor << ". Must be in (0,1].");
+ if (nextPoses.is_initialized()) {
return;
}
- ROS_INFO_STREAM("Moving to the next waypoint with speed factor " << motionSpeedFactor);
- TrajectoryUtil::applyMotionSpeedFactor(trajectory, motionSpeedFactor);
- moveit_msgs::MoveItErrorCodes errorCode = group.execute(trajectory);
- if (errorCode.val != errorCode.SUCCESS) {
- ROS_ERROR_STREAM("Error Code from executing segment: " << errorCode);
- }
- }
- } else {
- ROS_ERROR_STREAM("The planner was unable to compute a path to the pose with the given settings.");
- }
- // make sure the robot moves in an infinite circle
- if (i == (poses.poses.size() - 1)) {
- if (isLooping) {
- i = -1;
+ for (auto trajectory : split(group, plan)) {
+ double motionSpeedFactor;
+ if (!node_handle.getParam("robot_speed_factor", motionSpeedFactor)) {
+ motionSpeedFactor = default_velocity;
+ }
+ if (motionSpeedFactor <= 0 || motionSpeedFactor > 1.0) {
+ ROS_ERROR_STREAM("Invalid motion speed factor " << motionSpeedFactor << ". Must be in (0,1].");
+ return;
+ }
+ ROS_INFO_STREAM("Moving to the next waypoint with speed factor " << motionSpeedFactor);
+ TrajectoryUtil::applyMotionSpeedFactor(trajectory, motionSpeedFactor);
+ moveit_msgs::MoveItErrorCodes errorCode = group.execute(trajectory);
+ if (errorCode.val != errorCode.SUCCESS) {
+ ROS_ERROR_STREAM("Error Code from executing segment: " << errorCode);
+ }
+ }
+ } else {
+ ROS_ERROR_STREAM("The planner was unable to compute a path to the pose with the given settings.");
}
}
}
}
-void TimedPlanner::newPoseCallback(const geometry_msgs::PoseArray::ConstPtr &msg) {
- ROS_INFO_STREAM("Received new pose list with " << msg->poses.size() << " poses.");
+void TimedPlanner::newPoseCallback(const panda_mqtt_connector::Trajectory::ConstPtr &msg) {
+ ROS_INFO_STREAM("Received new pose list with " << msg->waypoints.size() << " poses.");
nextPoses = *msg;
}
@@ -171,8 +162,6 @@ TimedPlanner::TimedPlanner(moveit::planning_interface::MoveGroupInterface &group
ros::NodeHandle node_handle("panda_mqtt_connector");
- node_handle.getParam("loop_trajectory", isLooping);
-
std::string defaultTrajectory = "<none>";
node_handle.getParam("default_trajectory", defaultTrajectory);
@@ -200,22 +189,24 @@ void TimedPlanner::loadSquareTrajectory(geometry_msgs::Pose base_pose) {
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;
+ panda_mqtt_connector::Waypoint target_pose_1;
+ target_pose_1.pose = base_pose;
+ target_pose_1.mode = TrajectoryUtil::FLUID_PATH;
+ panda_mqtt_connector::Waypoint target_pose_2{target_pose_1};
+ panda_mqtt_connector::Waypoint target_pose_3{target_pose_1};
+ panda_mqtt_connector::Waypoint target_pose_4{target_pose_1};
- poses.poses.push_back(target_pose_1);
+ poses.waypoints.push_back(target_pose_1);
- target_pose_2.position.y *= -1;
- poses.poses.push_back(target_pose_2);
+ target_pose_2.pose.position.y *= -1;
+ poses.waypoints.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_3.pose.position.x *= -1;
+ target_pose_3.pose.position.y *= -1;
+ poses.waypoints.push_back(target_pose_3);
- target_pose_4.position.x *= -1;
- poses.poses.push_back(target_pose_4);
+ target_pose_4.pose.position.x *= -1;
+ poses.waypoints.push_back(target_pose_4);
}
diff --git a/src/TimedPlanner.h b/src/TimedPlanner.h
index ed0b03a..2031794 100644
--- a/src/TimedPlanner.h
+++ b/src/TimedPlanner.h
@@ -6,10 +6,11 @@
#define SRC_TIMEDPLANNER_H
-#include <geometry_msgs/PoseArray.h>
#include <boost/optional.hpp>
#include "util/TrajectoryUtil.h"
+#include "panda_mqtt_connector/Trajectory.h"
+
class TimedPlanner {
public:
@@ -24,13 +25,11 @@ public:
TimedPlanner(moveit::planning_interface::MoveGroupInterface &group, double defaultVelocity,
std::string defaultPlanningMode);
- void newPoseCallback(const geometry_msgs::PoseArray::ConstPtr &msg);
+ void newPoseCallback(const panda_mqtt_connector::Trajectory::ConstPtr &msg);
private:
- geometry_msgs::PoseArray poses;
- boost::optional<geometry_msgs::PoseArray> nextPoses;
- TrajectoryUtil traj_util;
- bool isLooping = true;
+ panda_mqtt_connector::Trajectory poses;
+ boost::optional<panda_mqtt_connector::Trajectory> nextPoses;
void loadCircularTrajectory(double radius, geometry_msgs::Point origin, int granularity);
diff --git a/src/util/TrajectoryUtil.cpp b/src/util/TrajectoryUtil.cpp
index cf71047..2566f1d 100644
--- a/src/util/TrajectoryUtil.cpp
+++ b/src/util/TrajectoryUtil.cpp
@@ -4,9 +4,6 @@
#include "TrajectoryUtil.h"
-const std::string TrajectoryUtil::CARTESIAN_PATH = "cartesian_path";
-const std::string TrajectoryUtil::FLUID_PATH = "fluid_path";
-
bool TrajectoryUtil::computePathToPose(moveit::planning_interface::MoveGroupInterface &group,
moveit::planning_interface::MoveGroupInterface::Plan &plan,
geometry_msgs::Pose targetPose,
diff --git a/src/util/TrajectoryUtil.h b/src/util/TrajectoryUtil.h
index fef5dab..4fd921b 100644
--- a/src/util/TrajectoryUtil.h
+++ b/src/util/TrajectoryUtil.h
@@ -18,17 +18,16 @@ class TrajectoryUtil {
public:
- static const std::string CARTESIAN_PATH;
- static const std::string FLUID_PATH;
+ constexpr static const char* const CARTESIAN_PATH = "cartesian_path";
+ constexpr static const char* const FLUID_PATH = "fluid_path";
- bool computePathToPose(moveit::planning_interface::MoveGroupInterface &group,
+ static bool computePathToPose(moveit::planning_interface::MoveGroupInterface &group,
moveit::planning_interface::MoveGroupInterface::Plan &plan,
geometry_msgs::Pose targetPose,
const std::string &pathType, double maxVelocityFactor,
double maxAccelerationFactor);
static void applyMotionSpeedFactor(moveit_msgs::RobotTrajectory &trajectory, double velocity);
-private:
};
--
GitLab