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

new protobuf definitions

parent c1b7b67f
Branches
No related tags found
No related merge requests found
......@@ -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
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 ##
......
# 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
# 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
......@@ -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>
......
......@@ -3,14 +3,10 @@ syntax = "proto3";
package config;
message RobotConfig {
double speed = 1;
bool loopTrajectory = 2;
enum PlanningMode {
FLUID = 0;
CARTESIAN = 1;
}
PlanningMode planningMode = 3;
message DataConfig {
repeated string links = 1;
double publishRate = 2;
}
\ No newline at end of file
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
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
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
......@@ -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
//
// 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");
......
#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());
void handleNewTrajectory(const plan::Trajectory &trajectory, const ros::Publisher &posePublisher) {
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_");
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 config::RobotConfig_PlanningMode_RobotConfig_PlanningMode_INT_MAX_SENTINEL_DO_NOT_USE_:
ROS_ERROR_STREAM("Planning-mode: INT_MAX_SENTINEL_DO_NOT_USE_");
case plan::Trajectory_PlanningMode_CARTESIAN:
waypoint.mode = TrajectoryUtil::CARTESIAN_PATH;
break;
}
default:
ROS_WARN("Received pose has invalid mode!");
}
void
handleNewTrajectory(const plan::Trajectory &trajectory, const ros::NodeHandle &n, const ros::Publisher &posePublisher) {
geometry_msgs::PoseArray poseArray;
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)) {
......
......@@ -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,26 +35,12 @@ 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 >>>");
for (int i = 0; i < msg.pose.size(); i++) {
......@@ -62,9 +48,6 @@ namespace robot_state_provider {
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 >>>");
......@@ -74,18 +57,14 @@ namespace robot_state_provider {
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);
}
}
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);
......@@ -93,74 +72,45 @@ namespace robot_state_provider {
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)) {
......
......@@ -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,21 +113,16 @@ 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;
if (traj_util.computePathToPose(group, plan, poses.poses.at(i), planning_mode, 0.8,
0.8)) {
ROS_ERROR_STREAM(waypoint);
if (TrajectoryUtil::computePathToPose(group, plan, waypoint.pose, waypoint.mode, 0.8, 0.8)) {
if (nextPoses.is_initialized()) {
return;
......@@ -150,18 +147,12 @@ void TimedPlanner::doMotion(const ros::NodeHandle &node_handle, moveit::planning
} 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;
}
}
}
}
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);
}
......
......@@ -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);
......
......@@ -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,
......
......@@ -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:
};
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment