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

rename variables and remove legacy code

parent 1bd83a4a
Branches
Tags
No related merge requests found
Show whitespace changes
Inline Side-by-side
src/MqttToRosNode.cpp
+ 10
− 10
View file @ f61d85fd
......@@ -23,11 +23,11 @@ void handleRobotConfig(const config::RobotConfig &robotConfig, const ros::NodeHa
n.setParam("robot_speed_factor", robotConfig.speed());
}
void handleNewTrajectory(const plan::Trajectory &trajectory, const ros::Publisher &posePublisher) {
void handleNewTrajectory(const plan::Trajectory &protoTrajectory, const ros::Publisher &trajectoryPublisher) {
panda_mqtt_connector::Trajectory poseArray;
panda_mqtt_connector::Trajectory trajectory;
for (const auto &protoPose : trajectory.pose()) {
for (const auto &protoPose : protoTrajectory.pose()) {
panda_mqtt_connector::Waypoint waypoint;
waypoint.pose.orientation.w = protoPose.orientation().w();
waypoint.pose.orientation.x = protoPose.orientation().x();
......@@ -48,16 +48,16 @@ void handleNewTrajectory(const plan::Trajectory &trajectory, const ros::Publishe
ROS_WARN("Received pose has invalid mode!");
}
poseArray.waypoints.push_back(waypoint);
trajectory.waypoints.push_back(waypoint);
}
poseArray.loop = trajectory.loop();
trajectory.loop = protoTrajectory.loop();
posePublisher.publish(poseArray);
trajectoryPublisher.publish(trajectory);
}
void receiveMqttMessages(const ros::NodeHandle &n, const ros::Publisher &posePublisher) {
void receiveMqttMessages(const ros::NodeHandle &n, const ros::Publisher &trajectoryPublisher) {
if (mqttUtil->ensureConnection()) {
mqtt::const_message_ptr msg;
if (mqttUtil->getClient().try_consume_message_for(&msg, std::chrono::milliseconds(500))) {
......@@ -71,7 +71,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, posePublisher);
handleNewTrajectory(trajectoryConfig, trajectoryPublisher);
}
}
} else {
......@@ -83,7 +83,7 @@ int main(int argc, char *argv[]) {
ros::init(argc, argv, "mqtt_listener");
ros::NodeHandle n("panda_mqtt_connector");
ros::Publisher posePublisher = n.advertise<panda_mqtt_connector::Trajectory>("poses", 1000);
ros::Publisher trajectoryPublisher = n.advertise<panda_mqtt_connector::Trajectory>("trajectory_update", 1000);
std::string server;
if (!n.getParam("server", server)) {
......@@ -111,7 +111,7 @@ int main(int argc, char *argv[]) {
}
while (ros::ok()) {
receiveMqttMessages(n, posePublisher);
receiveMqttMessages(n, trajectoryPublisher);
ros::spinOnce();
}
......
src/RobotStateProvider.cpp
+ 0
− 47
View file @ f61d85fd
......@@ -14,7 +14,6 @@
#include "util/MqttUtil.h"
namespace robot_state_provider {
bool export_to_log = false;
/**
* A map from the ROS element name to an MQTT topic
......@@ -30,49 +29,6 @@ namespace robot_state_provider {
MqttUtil *mqttUtil = nullptr;
/*
* logs to its own file in /.ros/logs (configured in launch-file)
*/
void exportMessageToLog(const gazebo_msgs::LinkStates &msg) {
ROS_INFO_STREAM("<<< LINK NAMES >>>");
for (int i = 0; i < msg.name.size(); i++) {
ROS_INFO_STREAM("[" << i << "] " << msg.name.at(i));
}
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);
}
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);
}
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("<<< 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<robot::RobotState> buildLinkStateMessages(const gazebo_msgs::LinkStates &msg) {
std::vector<robot::RobotState> messages{};
......@@ -159,9 +115,6 @@ namespace robot_state_provider {
}
if (current_redirect == 0) {
if (export_to_log) {
exportMessageToLog(msg);
}
sendMqttMessages(msg);
}
current_redirect = (current_redirect + 1) % message_redirect_rate;
......
src/TimedPlanner.cpp
+ 53
− 56
View file @ f61d85fd
......@@ -50,7 +50,7 @@ constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects, dou
void TimedPlanner::loadCircularTrajectory(double radius, geometry_msgs::Point origin, int granularity) {
nextPoses = panda_mqtt_connector::Trajectory{};
nextTrajectory = panda_mqtt_connector::Trajectory{};
double step_size_rad = 2 * M_PI / granularity;
......@@ -63,21 +63,22 @@ void TimedPlanner::loadCircularTrajectory(double radius, geometry_msgs::Point or
tf2::Quaternion orientation; // the RPY constructor is deprecated
orientation.setRPY(0, M_PI, next_rad);
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;
panda_mqtt_connector::Waypoint nextWaypoint{};
nextWaypoint.pose.orientation = tf2::toMsg(orientation);
nextWaypoint.pose.position.x = origin.x + radius * sin(next_rad);
nextWaypoint.pose.position.y = origin.y + radius * cos(next_rad);
nextWaypoint.pose.position.z = origin.z;
nextWaypoint.mode = TrajectoryUtil::FLUID_PATH;
nextPoses->waypoints.push_back(nextPose);
nextTrajectory->waypoints.push_back(nextWaypoint);
}
}
bool TimedPlanner::loadWaypoints() {
if (nextPoses.is_initialized()) {
poses = nextPoses.get();
nextPoses.reset();
if (nextTrajectory.is_initialized()) {
currentTrajectory = nextTrajectory.get();
nextTrajectory.reset();
return true;
} else {
return false;
......@@ -113,18 +114,14 @@ void TimedPlanner::doMotion(const ros::NodeHandle &node_handle, moveit::planning
loadWaypoints();
while (poses.loop) {
for (auto &waypoint : poses.waypoints) {
ROS_INFO_STREAM("Planning the next trajectory in " << waypoint.mode);
while (currentTrajectory.loop) {
for (auto &waypoint : currentTrajectory.waypoints) {
moveit::planning_interface::MoveGroupInterface::Plan plan;
ROS_ERROR_STREAM(waypoint);
if (TrajectoryUtil::computePathToPose(group, plan, waypoint.pose, waypoint.mode, 0.8, 0.8)) {
if (nextPoses.is_initialized()) {
if (nextTrajectory.is_initialized()) {
return;
}
......@@ -137,7 +134,8 @@ void TimedPlanner::doMotion(const ros::NodeHandle &node_handle, moveit::planning
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);
ROS_INFO_STREAM(
"Moving to the next waypoint with speed factor " << motionSpeedFactor << " in mode " << waypoint.mode);
TrajectoryUtil::applyMotionSpeedFactor(trajectory, motionSpeedFactor);
moveit_msgs::MoveItErrorCodes errorCode = group.execute(trajectory);
if (errorCode.val != errorCode.SUCCESS) {
......@@ -145,15 +143,16 @@ 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.");
ROS_ERROR_STREAM(
"Unable to compute " << waypoint.mode << " path to the next waypoint." << std::endl << waypoint);
}
}
}
}
void TimedPlanner::newPoseCallback(const panda_mqtt_connector::Trajectory::ConstPtr &msg) {
ROS_INFO_STREAM("Received new pose list with " << msg->waypoints.size() << " poses.");
nextPoses = *msg;
void TimedPlanner::newTrajectoryCallback(const panda_mqtt_connector::Trajectory::ConstPtr &msg) {
ROS_INFO_STREAM("Received new trajectory with " << msg->waypoints.size() << " waypoints.");
nextTrajectory = *msg;
}
TimedPlanner::TimedPlanner(moveit::planning_interface::MoveGroupInterface &group, const double defaultVelocity,
......@@ -162,51 +161,48 @@ TimedPlanner::TimedPlanner(moveit::planning_interface::MoveGroupInterface &group
ros::NodeHandle node_handle("panda_mqtt_connector");
std::string defaultTrajectory = "<none>";
std::string defaultTrajectory = "<no value provided>";
node_handle.getParam("default_trajectory", defaultTrajectory);
geometry_msgs::Pose base_pose = group.getCurrentPose().pose;
if (loadWaypoints()) {
ROS_INFO_STREAM("Ignoring default waypoints because poses were already received as a message");
} else if (defaultTrajectory == "circle") {
if (defaultTrajectory == "circle") {
ROS_INFO_STREAM("loading default trajectory 'circle'");
geometry_msgs::Point origin;
origin.z = .3;
loadCircularTrajectory(0.6, origin, 20);
} else if (defaultTrajectory == "square") {
loadSquareTrajectory(base_pose);
ROS_INFO_STREAM("loading default trajectory 'square'");
loadSquareTrajectory();
} else {
ROS_WARN_STREAM("not loading a default trajectory! provided trajectory: " + defaultTrajectory);
ROS_WARN_STREAM("Not loading a default trajectory! There is no '" + defaultTrajectory + "'.");
}
}
void TimedPlanner::loadSquareTrajectory(geometry_msgs::Pose base_pose) {
ROS_INFO_STREAM("loading default trajectory 'square'");
// choose a default trajectory
base_pose.position.x = 0.4;
base_pose.position.y = 0.4;
base_pose.position.z = 0.3;
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};
void TimedPlanner::loadSquareTrajectory() {
panda_mqtt_connector::Waypoint waypoint_1;
waypoint_1.pose.position.x = 0.4;
waypoint_1.pose.position.y = 0.4;
waypoint_1.pose.position.z = 0.3;
waypoint_1.pose.orientation.w = 0;
waypoint_1.pose.orientation.x = 0.7071067811865476;
waypoint_1.pose.orientation.y = 0.7071067811865476;
waypoint_1.pose.orientation.z = 0;
waypoint_1.mode = TrajectoryUtil::FLUID_PATH;
panda_mqtt_connector::Waypoint waypoint_2{waypoint_1};
panda_mqtt_connector::Waypoint waypoint_3{waypoint_1};
panda_mqtt_connector::Waypoint waypoint_4{waypoint_1};
poses.waypoints.push_back(target_pose_1);
currentTrajectory.waypoints.push_back(waypoint_1);
target_pose_2.pose.position.y *= -1;
poses.waypoints.push_back(target_pose_2);
waypoint_2.pose.position.y *= -1;
currentTrajectory.waypoints.push_back(waypoint_2);
target_pose_3.pose.position.x *= -1;
target_pose_3.pose.position.y *= -1;
poses.waypoints.push_back(target_pose_3);
waypoint_3.pose.position.x *= -1;
waypoint_3.pose.position.y *= -1;
currentTrajectory.waypoints.push_back(waypoint_3);
target_pose_4.pose.position.x *= -1;
poses.waypoints.push_back(target_pose_4);
waypoint_4.pose.position.x *= -1;
currentTrajectory.waypoints.push_back(waypoint_4);
}
......@@ -228,7 +224,8 @@ int main(int argc, char **argv) {
TimedPlanner planner(group, .5, TrajectoryUtil::FLUID_PATH);
ros::Subscriber sub = node_handle.subscribe("poses", 1000, &TimedPlanner::newPoseCallback, &planner);
ros::Subscriber sub = node_handle.subscribe("trajectory_update", 1000, &TimedPlanner::newTrajectoryCallback,
&planner);
// add the ground plate
std::vector<moveit_msgs::CollisionObject> collision_objects;
......
src/TimedPlanner.h
+ 12
− 11
View file @ f61d85fd
......@@ -11,31 +11,32 @@
#include "panda_mqtt_connector/Trajectory.h"
using moveit::planning_interface::MoveGroupInterface;
class TimedPlanner {
public:
const double default_velocity = 1.0;
const std::string default_planning_mode = TrajectoryUtil::FLUID_PATH;
static std::vector<moveit_msgs::RobotTrajectory> split(moveit::planning_interface::MoveGroupInterface &group,
const moveit::planning_interface::MoveGroupInterface::Plan &plan);
static std::vector<moveit_msgs::RobotTrajectory>
split(MoveGroupInterface &group, const MoveGroupInterface::Plan &plan);
void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::MoveGroupInterface &group);
void doMotion(const ros::NodeHandle &node_handle, MoveGroupInterface &group);
TimedPlanner(moveit::planning_interface::MoveGroupInterface &group, double defaultVelocity,
std::string defaultPlanningMode);
TimedPlanner(MoveGroupInterface &group, double defaultVelocity, std::string defaultPlanningMode);
void newPoseCallback(const panda_mqtt_connector::Trajectory::ConstPtr &msg);
void newTrajectoryCallback(const panda_mqtt_connector::Trajectory::ConstPtr &msg);
private:
panda_mqtt_connector::Trajectory poses;
boost::optional<panda_mqtt_connector::Trajectory> nextPoses;
void loadCircularTrajectory(double radius, geometry_msgs::Point origin, int granularity);
panda_mqtt_connector::Trajectory currentTrajectory;
boost::optional<panda_mqtt_connector::Trajectory> nextTrajectory;
bool loadWaypoints();
void loadSquareTrajectory(geometry_msgs::Pose base_pose);
void loadSquareTrajectory();
void loadCircularTrajectory(double radius, geometry_msgs::Point origin, int granularity);
};
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment