Skip to content
Snippets Groups Projects
Commit 8cdb2046 authored by Sebastian Ebert's avatar Sebastian Ebert
Browse files

upgrade to protobuff 3

parent 042a7f96
No related branches found
No related tags found
No related merge requests found
syntax = "proto2"; syntax = "proto3";
package config; package config;
message DataConfig { message DataConfig {
required bool enablePosition = 1; bool enablePosition = 1;
required bool enableOrientation = 2; bool enableOrientation = 2;
required bool enableTwistLinear = 3; bool enableTwistLinear = 3;
required bool enableTwistAngular = 4; bool enableTwistAngular = 4;
required int32 publishRate = 5; int32 publishRate = 5;
} }
\ No newline at end of file
syntax = "proto2"; syntax = "proto3";
package panda; package panda;
message PandaLinkState { message PandaLinkState {
required string name = 1; string name = 1;
message Position { message Position {
required float positionX = 1; float positionX = 1;
required float positionY = 2; float positionY = 2;
required float positionZ = 3; float positionZ = 3;
} }
message Orientation { message Orientation {
required float orientationX = 1; float orientationX = 1;
required float orientationY = 2; float orientationY = 2;
required float orientationZ = 3; float orientationZ = 3;
required float orientationW = 4; float orientationW = 4;
} }
message TwistLinear { message TwistLinear {
required float twistLinearX = 1; float twistLinearX = 1;
required float twistLinearY = 2; float twistLinearY = 2;
required float twistLinearZ = 3; float twistLinearZ = 3;
} }
message TwistAngular { message TwistAngular {
required float twistAngularX = 1; float twistAngularX = 1;
required float twistAngularY = 2; float twistAngularY = 2;
required float twistAngularZ = 3; float twistAngularZ = 3;
} }
optional Position pos = 2; Position pos = 2;
optional Orientation orient = 3; Orientation orient = 3;
optional TwistLinear tl = 4; TwistLinear tl = 4;
optional TwistAngular ta = 5; TwistAngular ta = 5;
} }
\ No newline at end of file
syntax = "proto2"; syntax = "proto3";
package config; package config;
message RobotConfig { message RobotConfig {
required double speed = 1; double speed = 1;
} }
\ No newline at end of file
...@@ -46,14 +46,14 @@ void handleDataConfig(config::DataConfig dataConfig, ros::NodeHandle n) ...@@ -46,14 +46,14 @@ void handleDataConfig(config::DataConfig dataConfig, ros::NodeHandle n)
<< " -- enablePosition: " << dataConfig.enableposition() << " -- enablePosition: " << dataConfig.enableposition()
<< " -- enableOrientation: " << dataConfig.enableorientation() << " -- enableOrientation: " << dataConfig.enableorientation()
<< " -- enableTwistLinear: " << dataConfig.enabletwistlinear() << " -- enableTwistLinear: " << dataConfig.enabletwistlinear()
<< " -- enableTwistAngular: " << dataConfig.has_enabletwistangular() << " -- enableTwistAngular: " << dataConfig.enabletwistangular()
<< std::endl; << std::endl;
n.setParam("data_publish_rate", dataConfig.publishrate()); n.setParam("data_publish_rate", dataConfig.publishrate());
n.setParam("data_enable_position", dataConfig.enableposition()); n.setParam("data_enable_position", dataConfig.enableposition());
n.setParam("data_enable_orientation", dataConfig.enableorientation()); n.setParam("data_enable_orientation", dataConfig.enableorientation());
n.setParam("data_enable_twist_linear", dataConfig.enabletwistlinear()); n.setParam("data_enable_twist_linear", dataConfig.enabletwistlinear());
n.setParam("data_enable_twist_angular", dataConfig.has_enabletwistangular()); n.setParam("data_enable_twist_angular", dataConfig.enabletwistangular());
} }
void receiveMqttMessages(ros::NodeHandle n) void receiveMqttMessages(ros::NodeHandle n)
......
...@@ -16,6 +16,25 @@ const double default_velocity = 1.0; ...@@ -16,6 +16,25 @@ const double default_velocity = 1.0;
bool isInitialized = false; bool isInitialized = false;
// adjust velocities, accelerations (= sqrt of velocity) and time_from_start
void manipulateVelocity(double velocity, moveit_msgs::RobotTrajectory &trajectory_msg) {
for (int i = 0; i < trajectory_msg.joint_trajectory.points.size(); i++) {
double new_tfs = 1 / velocity;
trajectory_msg.joint_trajectory.points[i].time_from_start.operator*=(new_tfs);
for (int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).velocities.size(); j++) {
if (trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) != 0.0) {
trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) *= velocity;
}
}
for (int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).accelerations.size(); j++) {
trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j) =
sqrt(std::abs(trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j)));
}
}
}
void initRawTrajectory(ros::NodeHandle node_handle, moveit::planning_interface::MoveGroupInterface *group) { void initRawTrajectory(ros::NodeHandle node_handle, moveit::planning_interface::MoveGroupInterface *group) {
geometry_msgs::Pose target_pose_1 = group->getCurrentPose().pose; geometry_msgs::Pose target_pose_1 = group->getCurrentPose().pose;
geometry_msgs::Pose target_pose_2 = group->getCurrentPose().pose; geometry_msgs::Pose target_pose_2 = group->getCurrentPose().pose;
...@@ -84,7 +103,6 @@ void computeTrajectory(moveit::planning_interface::MoveGroupInterface &group, in ...@@ -84,7 +103,6 @@ void computeTrajectory(moveit::planning_interface::MoveGroupInterface &group, in
bool success = iptp.computeTimeStamps(rt); bool success = iptp.computeTimeStamps(rt);
ROS_INFO("Computed time stamp %s", success ? "SUCCEDED" : "FAILED"); ROS_INFO("Computed time stamp %s", success ? "SUCCEDED" : "FAILED");
rt.getRobotTrajectoryMsg(trajectory_msg); rt.getRobotTrajectoryMsg(trajectory_msg);
plan.trajectory_ = trajectory_msg;
ROS_INFO("Visualizing cartesian plan (%.2f%% acheived)", fraction * 100.0); ROS_INFO("Visualizing cartesian plan (%.2f%% acheived)", fraction * 100.0);
} }
...@@ -111,6 +129,7 @@ int main(int argc, char **argv) { ...@@ -111,6 +129,7 @@ int main(int argc, char **argv) {
text_pose.translation().z() = 1.75; text_pose.translation().z() = 1.75;
visual_tools.publishText(text_pose, "cartesian planner node", rvt::WHITE, rvt::XLARGE); visual_tools.publishText(text_pose, "cartesian planner node", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger(); visual_tools.trigger();
//planning_scene::PlanningScene;
// Initialize start state of robot and target trajectory. // Initialize start state of robot and target trajectory.
moveit::planning_interface::MoveGroupInterface group("panda_arm"); moveit::planning_interface::MoveGroupInterface group("panda_arm");
...@@ -131,24 +150,10 @@ int main(int argc, char **argv) { ...@@ -131,24 +150,10 @@ int main(int argc, char **argv) {
moveit::planning_interface::MoveGroupInterface::Plan plan; moveit::planning_interface::MoveGroupInterface::Plan plan;
moveit_msgs::RobotTrajectory trajectory_msg; moveit_msgs::RobotTrajectory trajectory_msg;
computeTrajectory(group, i, plan, trajectory_msg); computeTrajectory(group, i, plan, trajectory_msg);
manipulateVelocity(velocity, trajectory_msg);
// adjust velocities, accelerations (= sqrt of velocity) and time_from_start
for (int i = 0; i < trajectory_msg.joint_trajectory.points.size(); i++) {
double new_tfs = 1 / velocity;
trajectory_msg.joint_trajectory.points[i].time_from_start.operator*=(new_tfs);
for (int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).velocities.size(); j++) {
if (trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) != 0.0) {
trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) *= velocity;
}
}
for (int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).accelerations.size(); j++) {
trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j) =
sqrt(std::abs(trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j)));
}
}
// execute one step of the trajectory // execute one step of the trajectory
plan.trajectory_ = trajectory_msg;
group.execute(plan); group.execute(plan);
// make sure the robot moves in an infinite circle // make sure the robot moves in an infinite circle
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment