yenon/ROS_Workspace
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

i should be learning

Browse Source 
yet i am refactoring
this is not a poem
pain.
This commit is contained in:
Bastian Hofmann 2022-03-27 01:05:50 +01:00
parent 435332c054
commit 2d2bc89a6b
23 changed files with 411 additions and 667 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/btree_nodes/CMakeLists.txt
View File

@ -18,7 +18,7 @@ find_package(behaviortree_cpp_v3 REQUIRED)
# further dependencies manually.
# find_package(<dependency> REQUIRED)
add_executable(tree src/Tree.cpp src/Extensions.cpp src/WeightedRandomNode.cpp)
add_executable(tree src/Tree.cpp src/Extensions.cpp src/nodes/WeightedRandomNode.cpp src/nodes/AmICalled.cpp src/nodes/GenerateTarget2D.cpp src/nodes/MoveActorToTarget.cpp src/helpers/MinimalSubscriber.cpp src/nodes/RobotMove.cpp)
ament_target_dependencies(tree rclcpp geometry_msgs std_msgs behaviortree_cpp_v3)
#add_executable(talker src/publisher_member_function.cpp)

5
src/btree_nodes/src/GenerateNewPose.cpp
View File

@ -1,5 +0,0 @@
//
// Created by bastian on 21.02.22.
//
#include "GenerateNewPose.h"

14
src/btree_nodes/src/GenerateNewPose.h
View File

@ -1,14 +0,0 @@
//
// Created by bastian on 21.02.22.
//
#ifndef BUILD_GENERATENEWPOSE_H
#define BUILD_GENERATENEWPOSE_H
class GenerateNewPose {
};
#endif //BUILD_GENERATENEWPOSE_H

31
src/btree_nodes/src/MinimalSubscriber.cpp
View File

@ -1,31 +0,0 @@
//
// Created by bastian on 22.02.22.
//
#include <memory>
#include <geometry_msgs/msg/pose.hpp>
#include "rclcpp/rclcpp.hpp"
template<typename T>
class MinimalSubscriber : public rclcpp::Node {
public:
MinimalSubscriber(const std::string &node_name, const std::string &topic_name, std::function<void(T)> callback) :
Node(node_name) {
this->subscription_ = this->create_subscription<T>(
topic_name, 10, [callback](const T result) {
callback(result);
});
}
private:
typename rclcpp::Subscription<T>::SharedPtr subscription_;
};
template<typename T>
std::thread spinMinimalSubscriber(const std::string &node_name, const std::string &topic_name, std::function<void(T)> callback) {
auto subscriber = std::make_shared<MinimalSubscriber<T>>(node_name, topic_name, callback);
return std::thread([subscriber]() {
rclcpp::spin(subscriber);
rclcpp::shutdown();
});
}

203
src/btree_nodes/src/Tree.cpp
View File

@ -1,204 +1,15 @@
#include <chrono>
#include "Area.h"
#include <behaviortree_cpp_v3/action_node.h>
#include <rclcpp/rclcpp.hpp>
#include <random>
#include <std_msgs/msg/bool.hpp>
#include "MinimalSubscriber.cpp"
#include "WeightedRandomNode.h"
//-------------------------------------------------------------
// Simple Action to print a number
//-------------------------------------------------------------
#include "nodes/WeightedRandomNode.h"
#include "nodes/AmICalled.h"
#include "nodes/GenerateTarget2D.h"
#include "nodes/MoveActorToTarget.h"
#include "helpers/MinimalSubscriber.h"
using namespace BT;
class AmICalled: public ConditionNode
{
public:
AmICalled(const std::string& name, const NodeConfiguration& config):
ConditionNode(name,config)
{}
static PortsList providedPorts()
{
return { InputPort<bool>("called") };
}
NodeStatus tick() override
{
bool called;
if(!getInput("called",called)){
return NodeStatus::FAILURE;
}
if(called){
return NodeStatus::SUCCESS;
}
return NodeStatus::FAILURE;
}
};
typedef std::chrono::milliseconds Milliseconds;
class MoveActorToTarget : public StatefulActionNode {
public:
MoveActorToTarget(const std::string &name, const NodeConfiguration &config)
: StatefulActionNode(name, config) {}
static PortsList providedPorts() {
return {
InputPort<geometry_msgs::msg::Pose>("current"),
InputPort<geometry_msgs::msg::Pose>("target")
};
}
geometry_msgs::msg::Pose target;
NodeStatus onStart() override {
std::cout << "started moving" << std::endl;
rclcpp::Node node("targetPublisherNode");
auto publisher = node.create_publisher<geometry_msgs::msg::Pose>("targetPose", 10);
auto res = getInput<geometry_msgs::msg::Pose>("target", target);
if (!res) {
std::cout << "[ no target available ]" << std::endl;
std::cout << res.error() << std::endl;
return NodeStatus::FAILURE;
}
publisher->publish(target);
return NodeStatus::RUNNING;
}
NodeStatus onRunning() override {
geometry_msgs::msg::Pose current;
auto res = getInput<geometry_msgs::msg::Pose>("current", current);
if (!res) {
std::cout << "[ no current position available ]" << std::endl;
std::cout << res.error() << std::endl;
return NodeStatus::FAILURE;
}
double dX = target.position.x - current.position.x;
double dY = target.position.y - current.position.y;
auto distance = sqrt(dX * dX + dY * dY);
if (distance < 0.3) {
return NodeStatus::SUCCESS;
} else {
return NodeStatus::RUNNING;
}
}
void onHalted() override {
std::cout << "halted move" << std::endl;
rclcpp::Node node("targetPublisherNode");
auto publisher = node.create_publisher<geometry_msgs::msg::Pose>("targetPose", 10);
geometry_msgs::msg::Pose inf;
inf.position.x = HUGE_VAL;
inf.position.y = HUGE_VAL;
publisher->publish(inf);
}
};
// This class cannot guarantee valid results with every polygon, triangles are assumed to be a valid triangle-strip.
class GenerateTarget2D : public SyncActionNode {
Area area;
std::vector<double> weights;
std::mt19937_64 rng;
private:
double getArea(unsigned long id) {
Position2D a = area.triangles[id];
Position2D b = area.triangles[id + 1];
Position2D c = area.triangles[id + 2];
return 0.5 * ((b.x - a.x) * (c.y - a.y) - (c.x - a.x) * (b.y - a.y));
}
private:
double getRandom() {
std::uniform_real_distribution<double> zeroToOne(0, 1);
return zeroToOne(rng);
}
private:
Position2D getRandomPosInTriangle(unsigned long id) {
Position2D a = area.triangles[id];
Position2D b = area.triangles[id + 1];
Position2D c = area.triangles[id + 2];
Position2D ab = {b.x - a.x, b.y - a.y};
Position2D ac = {c.x - a.x, c.y - a.y};
double u1 = getRandom();
double u2 = getRandom();
if (u1 + u2 > 1) {
u1 = 1 - u1;
u2 = 1 - u2;
}
return {a.x + (u1 * ab.x + u2 * ac.x), a.y + (u1 * ab.y + u2 * ac.y)};
}
public:
GenerateTarget2D(const std::string &name, const NodeConfiguration &config, const Area area) :
SyncActionNode(name, config) {
this->area = area;
unsigned long triangleCount = area.triangles.size() - 2;
uint64_t timeSeed = std::chrono::high_resolution_clock::now().time_since_epoch().count();
std::seed_seq ss{uint32_t(timeSeed & 0xffffffff), uint32_t(timeSeed >> 32)};
rng.seed(ss);
std::vector<double> prob(triangleCount);
double sum = 0;
for (unsigned long i = 0; i < triangleCount; ++i) {
double currentArea = getArea(i);
prob[i] = currentArea;
sum += currentArea;
}
for (unsigned long i = 0; i < triangleCount; ++i) {
prob[i] = prob[i] / sum;
}
sum = 0;
for (unsigned long i = 0; i < triangleCount; ++i) {
prob[i] = prob[i] + sum;
sum += prob[i];
}
this->weights = prob;
}
static PortsList providedPorts() {
// Optionally, a port can have a human readable description
const char *description = "Generates a random position in an area defined by a triangle strip";
return {OutputPort<geometry_msgs::msg::Pose>("target", description)};
}
NodeStatus tick() override {
double triangleRandom = getRandom();
unsigned long i = 0;
while (triangleRandom < weights[i + 1] && i < weights.size() - 1) {
i++;
}
auto pos = getRandomPosInTriangle(i);
geometry_msgs::msg::Pose randomPose;
randomPose.position.x=pos.x;
randomPose.position.y=pos.y;
printf("Generated Target: %f %f\n",pos.x,pos.y);
setOutput<geometry_msgs::msg::Pose>("target", randomPose);
return NodeStatus::SUCCESS;
}
};
int main(int argc, char **argv) {
std::cout << "Registering nodes." << std::endl;
@ -247,14 +58,14 @@ int main(int argc, char **argv) {
tree.rootBlackboard()->set<geometry_msgs::msg::Pose>("target",init);
tree.rootBlackboard()->set<bool>("called",false);
spinMinimalSubscriber<geometry_msgs::msg::Pose>("tree_get_currentPose", "currentPose",
MinimalSubscriber<geometry_msgs::msg::Pose>::createAsThread("tree_get_currentPose", "currentPose",
[&tree, &mutex](geometry_msgs::msg::Pose pose) {
mutex.lock();
tree.rootBlackboard()->set("current", pose);
mutex.unlock();
}).detach();
spinMinimalSubscriber<std_msgs::msg::Bool>("tree_get_called", "called",
MinimalSubscriber<std_msgs::msg::Bool>::createAsThread("tree_get_called", "called",
[&tree, &mutex](std_msgs::msg::Bool called) {
mutex.lock();
tree.rootBlackboard()->set("called", (bool) called.data);

6
src/btree_nodes/src/helpers/MinimalSubscriber.cpp Normal file
View File

@ -0,0 +1,6 @@
//
// Created by bastian on 26.03.22.
//
#include "MinimalSubscriber.h"

46
src/btree_nodes/src/helpers/MinimalSubscriber.h Normal file
View File

@ -0,0 +1,46 @@
//
// Created by bastian on 26.03.22.
//
#ifndef BUILD_MINIMALSUBSCRIBER_H
#define BUILD_MINIMALSUBSCRIBER_H
#include <memory>
#include <geometry_msgs/msg/pose.hpp>
#include "rclcpp/rclcpp.hpp"
template<typename T>
class MinimalSubscriber : public rclcpp::Node {
public:
MinimalSubscriber(const std::string &node_name, const std::string &topic_name,
std::function<void(T)> callback);
public:
static std::thread createAsThread(const std::string &node_name, const std::string &topic_name,
std::function<void(T)> callback);
private:
typename rclcpp::Subscription<T>::SharedPtr subscription_;
};
template<typename T>
MinimalSubscriber<T>::MinimalSubscriber(const std::string &node_name, const std::string &topic_name,
std::function<void(T)> callback) :
Node(node_name) {
this->subscription_ = this->create_subscription<T>(
topic_name, 10, [callback](const T result) {
callback(result);
});
}
template<typename T>
std::thread MinimalSubscriber<T>::createAsThread(const std::string &node_name, const std::string &topic_name,
std::function<void(T)> callback) {
auto subscriber = std::make_shared<MinimalSubscriber<T>>(node_name, topic_name, callback);
return std::thread([subscriber]() {
rclcpp::spin(subscriber);
rclcpp::shutdown();
});
}
#endif //BUILD_MINIMALSUBSCRIBER_H

23
src/btree_nodes/src/nodes/AmICalled.cpp Normal file
View File

@ -0,0 +1,23 @@
//
// Created by bastian on 26.03.22.
//
#include "AmICalled.h"
BT::AmICalled::AmICalled(const std::string &name, const BT::NodeConfiguration &config) :
ConditionNode(name, config) {}
BT::PortsList BT::AmICalled::providedPorts() {
return {InputPort<bool>("called")};
}
BT::NodeStatus BT::AmICalled::tick() {
bool called;
if (!getInput("called", called)) {
return NodeStatus::FAILURE;
}
if (called) {
return NodeStatus::SUCCESS;
}
return NodeStatus::FAILURE;
}

22
src/btree_nodes/src/nodes/AmICalled.h Normal file
View File

@ -0,0 +1,22 @@
//
// Created by bastian on 26.03.22.
//
#ifndef BUILD_AMICALLED_H
#define BUILD_AMICALLED_H
#include <behaviortree_cpp_v3/condition_node.h>
namespace BT {
class AmICalled : public ConditionNode {
public:
AmICalled(const std::string &name, const NodeConfiguration &config);
static PortsList providedPorts();
NodeStatus tick() override;
};
}
#endif //BUILD_AMICALLED_H

88
src/btree_nodes/src/nodes/GenerateTarget2D.cpp Normal file
View File

@ -0,0 +1,88 @@
//
// Created by bastian on 26.03.22.
//
#include "GenerateTarget2D.h"
double BT::GenerateTarget2D::getArea(unsigned long id) {
Position2D a = area.triangles[id];
Position2D b = area.triangles[id + 1];
Position2D c = area.triangles[id + 2];
return 0.5 * ((b.x - a.x) * (c.y - a.y) - (c.x - a.x) * (b.y - a.y));
}
double BT::GenerateTarget2D::getRandom() {
std::uniform_real_distribution<double> zeroToOne(0, 1);
return zeroToOne(rng);
}
Position2D BT::GenerateTarget2D::getRandomPosInTriangle(unsigned long id) {
Position2D a = area.triangles[id];
Position2D b = area.triangles[id + 1];
Position2D c = area.triangles[id + 2];
Position2D ab = {b.x - a.x, b.y - a.y};
Position2D ac = {c.x - a.x, c.y - a.y};
double u1 = getRandom();
double u2 = getRandom();
if (u1 + u2 > 1) {
u1 = 1 - u1;
u2 = 1 - u2;
}
return {a.x + (u1 * ab.x + u2 * ac.x), a.y + (u1 * ab.y + u2 * ac.y)};
}
BT::GenerateTarget2D::GenerateTarget2D(const std::string &name, const BT::NodeConfiguration &config, const Area area) :
SyncActionNode(name, config) {
this->area = area;
unsigned long triangleCount = area.triangles.size() - 2;
uint64_t timeSeed = std::chrono::high_resolution_clock::now().time_since_epoch().count();
std::seed_seq ss{uint32_t(timeSeed & 0xffffffff), uint32_t(timeSeed >> 32)};
rng.seed(ss);
std::vector<double> prob(triangleCount);
double sum = 0;
for (unsigned long i = 0; i < triangleCount; ++i) {
double currentArea = getArea(i);
prob[i] = currentArea;
sum += currentArea;
}
for (unsigned long i = 0; i < triangleCount; ++i) {
prob[i] = prob[i] / sum;
}
sum = 0;
for (unsigned long i = 0; i < triangleCount; ++i) {
prob[i] = prob[i] + sum;
sum += prob[i];
}
this->weights = prob;
}
BT::PortsList BT::GenerateTarget2D::providedPorts() {
// Optionally, a port can have a human readable description
const char *description = "Generates a random position in an area defined by a triangle strip";
return {OutputPort<geometry_msgs::msg::Pose>("target", description)};
}
BT::NodeStatus BT::GenerateTarget2D::tick() {
double triangleRandom = getRandom();
unsigned long i = 0;
while (triangleRandom < weights[i + 1] && i < weights.size() - 1) {
i++;
}
auto pos = getRandomPosInTriangle(i);
geometry_msgs::msg::Pose randomPose;
randomPose.position.x=pos.x;
randomPose.position.y=pos.y;
printf("Generated Target: %f %f\n",pos.x,pos.y);
setOutput<geometry_msgs::msg::Pose>("target", randomPose);
return NodeStatus::SUCCESS;
}

40
src/btree_nodes/src/nodes/GenerateTarget2D.h Normal file
View File

@ -0,0 +1,40 @@
//
// Created by bastian on 26.03.22.
//
#ifndef BUILD_GENERATETARGET2D_H
#define BUILD_GENERATETARGET2D_H
#include <behaviortree_cpp_v3/action_node.h>
#include "../Area.h"
#include <random>
#include <geometry_msgs/msg/pose.hpp>
namespace BT{
// This class cannot guarantee valid results with every polygon, triangles are assumed to be a valid triangle-strip.
class GenerateTarget2D : public SyncActionNode {
Area area;
std::vector<double> weights;
std::mt19937_64 rng;
private:
double getArea(unsigned long id);
private:
double getRandom();
private:
Position2D getRandomPosInTriangle(unsigned long id);
public:
GenerateTarget2D(const std::string &name, const NodeConfiguration &config, const Area area);
static PortsList providedPorts();
NodeStatus tick() override;
};
}
#endif //BUILD_GENERATETARGET2D_H

64
src/btree_nodes/src/nodes/MoveActorToTarget.cpp Normal file
View File

@ -0,0 +1,64 @@
//
// Created by bastian on 26.03.22.
//
#include "MoveActorToTarget.h"
BT::MoveActorToTarget::MoveActorToTarget(const std::string &name, const BT::NodeConfiguration &config)
: StatefulActionNode(name, config) {}
BT::PortsList BT::MoveActorToTarget::providedPorts() {
return {
InputPort<geometry_msgs::msg::Pose>("current"),
InputPort<geometry_msgs::msg::Pose>("target")
};
}
BT::NodeStatus BT::MoveActorToTarget::onStart() {
std::cout << "started moving" << std::endl;
rclcpp::Node node("targetPublisherNode");
auto publisher = node.create_publisher<geometry_msgs::msg::Pose>("targetPose", 10);
auto res = getInput<geometry_msgs::msg::Pose>("target", target);
if (!res) {
std::cout << "[ no target available ]" << std::endl;
std::cout << res.error() << std::endl;
return NodeStatus::FAILURE;
}
publisher->publish(target);
return NodeStatus::RUNNING;
}
BT::NodeStatus BT::MoveActorToTarget::onRunning() {
geometry_msgs::msg::Pose current;
auto res = getInput<geometry_msgs::msg::Pose>("current", current);
if (!res) {
std::cout << "[ no current position available ]" << std::endl;
std::cout << res.error() << std::endl;
return NodeStatus::FAILURE;
}
double dX = target.position.x - current.position.x;
double dY = target.position.y - current.position.y;
auto distance = sqrt(dX * dX + dY * dY);
if (distance < 0.3) {
return NodeStatus::SUCCESS;
} else {
return NodeStatus::RUNNING;
}
}
void BT::MoveActorToTarget::onHalted() {
std::cout << "halted move" << std::endl;
rclcpp::Node node("targetPublisherNode");
auto publisher = node.create_publisher<geometry_msgs::msg::Pose>("targetPose", 10);
geometry_msgs::msg::Pose inf;
inf.position.x = HUGE_VAL;
inf.position.y = HUGE_VAL;
publisher->publish(inf);
}

30
src/btree_nodes/src/nodes/MoveActorToTarget.h Normal file
View File

@ -0,0 +1,30 @@
//
// Created by bastian on 26.03.22.
//
#ifndef BUILD_MOVEACTORTOTARGET_H
#define BUILD_MOVEACTORTOTARGET_H
#include <behaviortree_cpp_v3/action_node.h>
#include <geometry_msgs/msg/pose.hpp>
#include <rclcpp/rclcpp.hpp>
namespace BT {
class MoveActorToTarget : public StatefulActionNode {
public:
MoveActorToTarget(const std::string &name, const NodeConfiguration &config);
static PortsList providedPorts();
geometry_msgs::msg::Pose target;
NodeStatus onStart() override;
NodeStatus onRunning() override;
void onHalted() override;
};
}
#endif //BUILD_MOVEACTORTOTARGET_H

5
src/btree_nodes/src/nodes/RobotMove.cpp Normal file
View File

@ -0,0 +1,5 @@
//
// Created by bastian on 26.03.22.
//
#include "RobotMove.h"

22
src/btree_nodes/src/nodes/RobotMove.h Normal file
View File

@ -0,0 +1,22 @@
//
// Created by bastian on 26.03.22.
//
#ifndef BUILD_ROBOTMOVE_H
#define BUILD_ROBOTMOVE_H
#include <behaviortree_cpp_v3/action_node.h>
#include <geometry_msgs/msg/pose.hpp>
namespace BT {
class RobotMove : public StatefulActionNode {
public:
RobotMove(const std::string &name, const BT::NodeConfiguration &config)
: StatefulActionNode(name, config) {
}
};
}
#endif //BUILD_ROBOTMOVE_H

5
src/btree_nodes/src/WeightedRandomNode.cpp → src/btree_nodes/src/nodes/WeightedRandomNode.cpp
View File

@ -3,7 +3,6 @@
//
#include "WeightedRandomNode.h"
#include <regex>
WeightedRandomNode::WeightedRandomNode(const std::string &name, const NodeConfiguration &config)
: ControlNode::ControlNode(name, config), selected_child_index(0), select_next(true) {
@ -68,3 +67,7 @@ NodeStatus WeightedRandomNode::tick() {
return child_status;
}
PortsList WeightedRandomNode::providedPorts() {
return { InputPort<bool>("weights") };
}

5
src/btree_nodes/src/WeightedRandomNode.h → src/btree_nodes/src/nodes/WeightedRandomNode.h
View File

@ -8,6 +8,7 @@
#include <behaviortree_cpp_v3/control_node.h>
#include <string>
#include <random>
#include <regex>
using namespace BT;
@ -20,9 +21,7 @@ public:
virtual void halt() override;
static PortsList providedPorts(){
return { InputPort<bool>("weights") };
}
static PortsList providedPorts();
private:
size_t selected_child_index;
bool select_next{};

400
src/btree_robot/src/robot_test.cpp
View File

@ -37,14 +37,6 @@
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/msg/display_robot_state.hpp>
#include <moveit_msgs/msg/display_trajectory.hpp>
#include <moveit_msgs/msg/attached_collision_object.hpp>
#include <moveit_msgs/msg/collision_object.hpp>
#include <moveit_visual_tools/moveit_visual_tools.h>
// All source files that use ROS logging should define a file-specific
// static const rclcpp::Logger named LOGGER, located at the top of the file
// and inside the namespace with the narrowest scope (if there is one)
@ -87,43 +79,11 @@ int main(int argc, char** argv)
const moveit::core::JointModelGroup* joint_model_group =
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
// Visualization
// ^^^^^^^^^^^^^
namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools(move_group_node, "base_bottom", "iisy",
move_group.getRobotModel());
visual_tools.deleteAllMarkers();
/* Remote control is an introspection tool that allows users to step through a high level script */
/* via buttons and keyboard shortcuts in RViz */
visual_tools.loadRemoteControl();
// RViz provides many types of markers, in this demo we will use text, cylinders, and spheres
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.0;
visual_tools.publishText(text_pose, "MoveGroupInterface_Demo", rvt::WHITE, rvt::XLARGE);
// Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
visual_tools.trigger();
// Getting Basic Information
// ^^^^^^^^^^^^^^^^^^^^^^^^^
//
// We can print the name of the reference frame for this robot.
RCLCPP_INFO(LOGGER, "Planning frame: %s", move_group.getPlanningFrame().c_str());
// We can also print the name of the end-effector link for this group.
RCLCPP_INFO(LOGGER, "End effector link: %s", move_group.getEndEffectorLink().c_str());
// We can get a list of all the groups in the robot:
RCLCPP_INFO(LOGGER, "Available Planning Groups:");
std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
std::ostream_iterator<std::string>(std::cout, ", "));
// Start the demo
// ^^^^^^^^^^^^^^^^^^^^^^^^^
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");
// visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");
// .. _move_group_interface-planning-to-pose-goal:
//
@ -132,10 +92,12 @@ int main(int argc, char** argv)
// We can plan a motion for this group to a desired pose for the
// end-effector.
geometry_msgs::msg::Pose target_pose1;
target_pose1.orientation.w = 1.0;
target_pose1.position.x = 0.28;
target_pose1.orientation.w = 0;
target_pose1.orientation.y = 1;
target_pose1.position.x = 0.2;
target_pose1.position.y = -0.2;
target_pose1.position.z = 0.5;
target_pose1.position.z = 1.2;
move_group.setPoseTarget(target_pose1);
// Now, we call the planner to compute the plan and visualize it.
@ -147,354 +109,8 @@ int main(int argc, char** argv)
RCLCPP_INFO(LOGGER, "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
// Visualizing plans
// ^^^^^^^^^^^^^^^^^
// We can also visualize the plan as a line with markers in RViz.
RCLCPP_INFO(LOGGER, "Visualizing plan 1 as trajectory line");
visual_tools.publishAxisLabeled(target_pose1, "pose1");
visual_tools.publishText(text_pose, "Pose_Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
// Moving to a pose goal
// ^^^^^^^^^^^^^^^^^^^^^
//
// Moving to a pose goal is similar to the step above
// except we now use the ``move()`` function. Note that
// the pose goal we had set earlier is still active
// and so the robot will try to move to that goal. We will
// not use that function in this tutorial since it is
// a blocking function and requires a controller to be active
// and report success on execution of a trajectory.
/* Uncomment below line when working with a real robot */
/* move_group.move(); */
// Planning to a joint-space goal
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
//
// Let's set a joint space goal and move towards it. This will replace the
// pose target we set above.
//
// To start, we'll create an pointer that references the current robot's state.
// RobotState is the object that contains all the current position/velocity/acceleration data.
moveit::core::RobotStatePtr current_state = move_group.getCurrentState(10);
//
// Next get the current set of joint values for the group.
std::vector<double> joint_group_positions;
current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);
// Now, let's modify one of the joints, plan to the new joint space goal, and visualize the plan.
joint_group_positions[0] = -1.0; // radians
move_group.setJointValueTarget(joint_group_positions);
// We lower the allowed maximum velocity and acceleration to 5% of their maximum.
// The default values are 10% (0.1).
// Set your preferred defaults in the joint_limits.yaml file of your robot's moveit_config
// or set explicit factors in your code if you need your robot to move faster.
move_group.setMaxVelocityScalingFactor(0.05);
move_group.setMaxAccelerationScalingFactor(0.05);
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
RCLCPP_INFO(LOGGER, "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED");
// Visualize the plan in RViz:
visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Joint_Space_Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
// Planning with Path Constraints
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
//
// Path constraints can easily be specified for a link on the robot.
// Let's specify a path constraint and a pose goal for our group.
// First define the path constraint.
moveit_msgs::msg::OrientationConstraint ocm;
ocm.link_name = "link3_top";
ocm.header.frame_id = "base_bottom";
ocm.orientation.w = 1.0;
ocm.absolute_x_axis_tolerance = 0.1;
ocm.absolute_y_axis_tolerance = 0.1;
ocm.absolute_z_axis_tolerance = 0.1;
ocm.weight = 1.0;
// Now, set it as the path constraint for the group.
moveit_msgs::msg::Constraints test_constraints;
test_constraints.orientation_constraints.push_back(ocm);
move_group.setPathConstraints(test_constraints);
// Enforce Planning in Joint Space
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
//
// Depending on the planning problem MoveIt chooses between
// ``joint space`` and ``cartesian space`` for problem representation.
// Setting the group parameter ``enforce_joint_model_state_space:true`` in
// the ompl_planning.yaml file enforces the use of ``joint space`` for all plans.
//
// By default, planning requests with orientation path constraints
// are sampled in ``cartesian space`` so that invoking IK serves as a
// generative sampler.
//
// By enforcing ``joint space``, the planning process will use rejection
// sampling to find valid requests. Please note that this might
// increase planning time considerably.
//
// We will reuse the old goal that we had and plan to it.
// Note that this will only work if the current state already
// satisfies the path constraints. So we need to set the start
// state to a new pose.
moveit::core::RobotState start_state(*move_group.getCurrentState());
geometry_msgs::msg::Pose start_pose2;
start_pose2.orientation.w = 1.0;
start_pose2.position.x = 0.55;
start_pose2.position.y = -0.05;
start_pose2.position.z = 0.8;
start_state.setFromIK(joint_model_group, start_pose2);
move_group.setStartState(start_state);
// Now, we will plan to the earlier pose target from the new
// start state that we just created.
move_group.setPoseTarget(target_pose1);
// Planning with constraints can be slow because every sample must call an inverse kinematics solver.
// Let's increase the planning time from the default 5 seconds to be sure the planner has enough time to succeed.
move_group.setPlanningTime(10.0);
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
RCLCPP_INFO(LOGGER, "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED");
// Visualize the plan in RViz:
visual_tools.deleteAllMarkers();
visual_tools.publishAxisLabeled(start_pose2, "start");
visual_tools.publishAxisLabeled(target_pose1, "goal");
visual_tools.publishText(text_pose, "Constrained_Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
// When done with the path constraint, be sure to clear it.
move_group.clearPathConstraints();
// Cartesian Paths
// ^^^^^^^^^^^^^^^
// You can plan a Cartesian path directly by specifying a list of waypoints
// for the end-effector to go through. Note that we are starting
// from the new start state above. The initial pose (start state) does not
// need to be added to the waypoint list but adding it can help with visualizations
std::vector<geometry_msgs::msg::Pose> waypoints;
waypoints.push_back(start_pose2);
geometry_msgs::msg::Pose target_pose3 = start_pose2;
target_pose3.position.z -= 0.2;
waypoints.push_back(target_pose3); // down
target_pose3.position.y -= 0.2;
waypoints.push_back(target_pose3); // right
target_pose3.position.z += 0.2;
target_pose3.position.y += 0.2;
target_pose3.position.x -= 0.2;
waypoints.push_back(target_pose3); // up and left
// We want the Cartesian path to be interpolated at a resolution of 1 cm
// which is why we will specify 0.01 as the max step in Cartesian
// translation. We will specify the jump threshold as 0.0, effectively disabling it.
// Warning - disabling the jump threshold while operating real hardware can cause
// large unpredictable motions of redundant joints and could be a safety issue
moveit_msgs::msg::RobotTrajectory trajectory;
const double jump_threshold = 0.0;
const double eef_step = 0.01;
double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
RCLCPP_INFO(LOGGER, "Visualizing plan 4 (Cartesian path) (%.2f%% achieved)", fraction * 100.0);
// Visualize the plan in RViz
visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Cartesian_Path", rvt::WHITE, rvt::XLARGE);
visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL);
for (std::size_t i = 0; i < waypoints.size(); ++i)
visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
// Cartesian motions should often be slow, e.g. when approaching objects. The speed of Cartesian
// plans cannot currently be set through the maxVelocityScalingFactor, but requires you to time
// the trajectory manually, as described `here <https://groups.google.com/forum/#!topic/moveit-users/MOoFxy2exT4>`_.
// Pull requests are welcome.
//
// You can execute a trajectory like this.
/* move_group.execute(trajectory); */
// Adding objects to the environment
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
//
// First, let's plan to another simple goal with no objects in the way.
move_group.setStartState(*move_group.getCurrentState());
geometry_msgs::msg::Pose another_pose;
another_pose.orientation.w = 0;
another_pose.orientation.x = -1.0;
another_pose.position.x = 0.7;
another_pose.position.y = 0.0;
another_pose.position.z = 0.59;
move_group.setPoseTarget(another_pose);
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
RCLCPP_INFO(LOGGER, "Visualizing plan 5 (with no obstacles) %s", success ? "" : "FAILED");
visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Clear_Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishAxisLabeled(another_pose, "goal");
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
// The result may look like this:
//
// .. image:: ./move_group_interface_tutorial_clear_path.gif
// :alt: animation showing the arm moving relatively straight toward the goal
//
// Now, let's define a collision object ROS message for the robot to avoid.
moveit_msgs::msg::CollisionObject collision_object;
collision_object.header.frame_id = move_group.getPlanningFrame();
// The id of the object is used to identify it.
collision_object.id = "box1";
// Define a box to add to the world.
shape_msgs::msg::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[primitive.BOX_X] = 0.1;
primitive.dimensions[primitive.BOX_Y] = 1.5;
primitive.dimensions[primitive.BOX_Z] = 0.5;
// Define a pose for the box (specified relative to frame_id).
geometry_msgs::msg::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x = 0.48;
box_pose.position.y = 0.0;
box_pose.position.z = 0.25;
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
std::vector<moveit_msgs::msg::CollisionObject> collision_objects;
collision_objects.push_back(collision_object);
// Now, let's add the collision object into the world
// (using a vector that could contain additional objects)
RCLCPP_INFO(LOGGER, "Add an object into the world");
planning_scene_interface.addCollisionObjects(collision_objects);
// Show text in RViz of status and wait for MoveGroup to receive and process the collision object message
visual_tools.publishText(text_pose, "Add_object", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");
// Now, when we plan a trajectory it will avoid the obstacle.
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
RCLCPP_INFO(LOGGER, "Visualizing plan 6 (pose goal move around cuboid) %s", success ? "" : "FAILED");
visual_tools.publishText(text_pose, "Obstacle_Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete");
// The result may look like this:
//
// .. image:: ./move_group_interface_tutorial_avoid_path.gif
// :alt: animation showing the arm moving avoiding the new obstacle
//
// Attaching objects to the robot
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
//
// You can attach an object to the robot, so that it moves with the robot geometry.
// This simulates picking up the object for the purpose of manipulating it.
// The motion planning should avoid collisions between objects as well.
moveit_msgs::msg::CollisionObject object_to_attach;
object_to_attach.id = "cylinder1";
shape_msgs::msg::SolidPrimitive cylinder_primitive;
cylinder_primitive.type = primitive.CYLINDER;
cylinder_primitive.dimensions.resize(2);
cylinder_primitive.dimensions[primitive.CYLINDER_HEIGHT] = 0.20;
cylinder_primitive.dimensions[primitive.CYLINDER_RADIUS] = 0.04;
// We define the frame/pose for this cylinder so that it appears in the gripper.
object_to_attach.header.frame_id = move_group.getEndEffectorLink();
geometry_msgs::msg::Pose grab_pose;
grab_pose.orientation.w = 1.0;
grab_pose.position.z = 0.2;
// First, we add the object to the world (without using a vector).
object_to_attach.primitives.push_back(cylinder_primitive);
object_to_attach.primitive_poses.push_back(grab_pose);
object_to_attach.operation = object_to_attach.ADD;
planning_scene_interface.applyCollisionObject(object_to_attach);
// Then, we "attach" the object to the robot. It uses the frame_id to determine which robot link it is attached to.
// We also need to tell MoveIt that the object is allowed to be in collision with the finger links of the gripper.
// You could also use applyAttachedCollisionObject to attach an object to the robot directly.
RCLCPP_INFO(LOGGER, "Attach the object to the robot");
std::vector<std::string> touch_links;
touch_links.push_back("panda_rightfinger");
touch_links.push_back("panda_leftfinger");
move_group.attachObject(object_to_attach.id, "panda_hand", touch_links);
visual_tools.publishText(text_pose, "Object_attached_to_robot", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
/* Wait for MoveGroup to receive and process the attached collision object message */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is attached to the robot");
// Replan, but now with the object in hand.
move_group.setStartStateToCurrentState();
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
RCLCPP_INFO(LOGGER, "Visualizing plan 7 (move around cuboid with cylinder) %s", success ? "" : "FAILED");
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete");
// The result may look something like this:
//
// .. image:: ./move_group_interface_tutorial_attached_object.gif
// :alt: animation showing the arm moving differently once the object is attached
//
// Detaching and Removing Objects
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
//
// Now, let's detach the cylinder from the robot's gripper.
RCLCPP_INFO(LOGGER, "Detach the object from the robot");
move_group.detachObject(object_to_attach.id);
// Show text in RViz of status
visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Object_detached_from_robot", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
/* Wait for MoveGroup to receive and process the attached collision object message */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is detached from the robot");
// Now, let's remove the objects from the world.
RCLCPP_INFO(LOGGER, "Remove the objects from the world");
std::vector<std::string> object_ids;
object_ids.push_back(collision_object.id);
object_ids.push_back(object_to_attach.id);
planning_scene_interface.removeCollisionObjects(object_ids);
// Show text in RViz of status
visual_tools.publishText(text_pose, "Objects_removed", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
/* Wait for MoveGroup to receive and process the attached collision object message */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disappears");
// END_TUTORIAL
visual_tools.deleteAllMarkers();
visual_tools.trigger();
move_group.asyncExecute(my_plan.trajectory_);
move_group.execute(my_plan);
rclcpp::shutdown();
return 0;

42
src/gaz_simulation/launch/test_launch.py
View File

@ -42,30 +42,30 @@ def generate_launch_description():
name='lidar_2_broadcaster',
arguments=["-1.9", "1", "1", "0", "0", "0", "map", "lidar_2_link"]
),
Node(
package='gazebo_ros',
executable='spawn_entity.py',
arguments=['-entity', robot_name,
'-topic', 'robot_description',
'-x', '-1',
'-y', '-1',
'-z', '1',
'-Y', '0'], # Yaw
output='screen'
),
#Node(
# package='gazebo_ros',
# executable='spawn_entity.py',
# arguments=['-entity', robot_name,
# '-topic', 'robot_description',
# '-x', '-1',
# '-y', '-1',
# '-z', '1',
# '-Y', '0'], # Yaw
# output='screen'
#),
# Subscribe to the joint states of the robot, and publish the 3D pose of each link.
Node(
package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[{'robot_description': Command(['xacro ', urdf_path])}]
),
#Node(
# package='robot_state_publisher',
# executable='robot_state_publisher',
# parameters=[{'robot_description': Command(['xacro ', urdf_path])}]
#),
# Publish the joint states of the robot
Node(
package='joint_state_publisher',
executable='joint_state_publisher',
name='joint_state_publisher'
)
#Node(
# package='joint_state_publisher',
# executable='joint_state_publisher',
# name='joint_state_publisher'
#)
# ,
# Node(

2
src/iisy_config/launch/lbr_move_group.launch.py
View File

@ -149,6 +149,8 @@ def launch_setup(context, *args, **kwargs):
robot_description,
robot_description_semantic,
kinematics_yaml,
planning,
use_sim_time
],
)

1
src/iisy_config/srdf/iisy.srdf
View File

@ -27,6 +27,7 @@
<joint name="link3_rot" value="0"/>
</group_state>
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
<disable_collisions link1="table" link2="base_bottom" reason="Adjacent"/>
<disable_collisions link1="base_bottom" link2="base_top" reason="Adjacent"/>
<disable_collisions link1="base_top" link2="link1_full" reason="Adjacent"/>
<disable_collisions link1="base_top" link2="link2_bottom" reason="Never"/>

BIN
src/iisy_config/stl/table.stl Executable file
View File

Binary file not shown.

20
src/iisy_config/urdf/iisy.urdf
View File

@ -5,6 +5,18 @@
<link name="world"/>
<link name="base_link"/>
<link name="table">
<collision>
<geometry>
<mesh filename="file://$(find iisy_config)/stl/table.stl"/>
</geometry>
</collision>
<visual>
<geometry>
<mesh filename="file://$(find iisy_config)/stl/table.stl"/>
</geometry>
</visual>
</link>
<link name="base_bottom">
<collision>
<geometry>
@ -22,7 +34,6 @@
<mass value="6"/>
<inertia ixx="1.277905" ixy="0.0" ixz="0.138279" iyy="3.070705" iyz="0.0" izz="3.284356"/>
</inertial>
</link>
<link name="base_top">
<collision>
@ -138,8 +149,8 @@
<mass value="0.8"/>
<inertia ixx="0.228470" ixy="0.0" ixz="0.0" iyy="0.137641" iyz="0.0" izz="0.252003"/>
</inertial>
</link>
<joint type="fixed" name="world_base_link_fixed">
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="world"/>
@ -148,6 +159,11 @@
<joint type="fixed" name="base_base_link_fixed">
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="table"/>
</joint>
<joint type="fixed" name="table_link_fixed">
<origin xyz="0 0 1" rpy="0 0 0"/>
<parent link="table"/>
<child link="base_bottom"/>
</joint>
<joint type="continuous" name="base_rot">