Update virtual_box.cpp

rusty/config/location.yaml

@@ -0,0 +1,7 @@
+pick_up:
+  x: 10.0
+  w: 1.0
+
+drop:
+  x: -9.0
+  w: 1.0

rusty/launch/navigation.launch

@@ -19,7 +19,7 @@
 
     <arg name="move_forward_only" value="$(arg move_forward_only)"/>
   </include>
-
+  <rosparam command="load" file="$(find rusty)/config/location.yaml" />
   <!-- rviz -->
 
   <arg default="$(find rusty_description)/launch/rusty_display.rviz" name="rvizconfig"/>

rusty/params/dwa_local_planner_params.yaml

@@ -1,7 +1,7 @@
 DWAPlannerROS:
 
 # Robot Configuration Parameters
-  max_vel_x: 0.35
+  max_vel_x: 0.30
   min_vel_x: -0.20
 
   max_vel_y: 0.0
@@ -11,8 +11,8 @@ DWAPlannerROS:
   max_vel_trans:  0.35
   min_vel_trans:  0.13
 
-  max_vel_theta: 2.8
-  min_vel_theta: 1.5
+  max_vel_theta: 1.5
+  min_vel_theta: 0.4
 
   acc_lim_x: 2.5
   acc_lim_y: 0.0

rusty/src/pick_obj.cpp

@@ -1,62 +1,69 @@
 #include <ros/ros.h>
 #include <move_base_msgs/MoveBaseAction.h>
 #include <actionlib/client/simple_action_client.h>
-#include <iostream>
+#include <std_msgs/UInt8.h>
 
-// Define a client for to send goal requests to the move_base server through a SimpleActionClient
 typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
 
-float pose(float pose)
-{
-  MoveBaseClient ac("move_base", true);
-  //float pose_arr[2]={-15.0,10.0};
 
+int main(int argc, char** argv){
+  // Initialize the simple_navigation_goals node
+  ros::init(argc, argv, "pick_objects");
+  ros::NodeHandle n;
 
+  MoveBaseClient ac("move_base", true);
+  //set up publisher to broadcast if robot is at goal marker
+  ros::Publisher pub = n.advertise<std_msgs::UInt8>("/goal", 1);
   // Wait 5 sec for move_base action server to come up
-  while(!ac.waitForServer(ros::Duration(5.0)))
-  {
+  while(!ac.waitForServer(ros::Duration(5.0))){
     ROS_INFO("Waiting for the move_base action server to come up");
   }
 
-  move_base_msgs::MoveBaseGoal goal;
+  move_base_msgs::MoveBaseGoal goal;	
+  std_msgs::UInt8 delivery_data;
+  ROS_INFO("PARAM loaded");
   goal.target_pose.header.frame_id = "map";
   goal.target_pose.header.stamp = ros::Time::now();
-
-  ROS_INFO("GOING  TO POSITION %f",pose);
-  goal.target_pose.pose.position.x = pose;
-  goal.target_pose.pose.orientation.w = 1.0;
-
-
 
+  n.getParam("/pick_up/x", goal.target_pose.pose.position.x);
+  n.getParam("/pick_up/w", goal.target_pose.pose.orientation.w);
 
-   // Send the goal position and orientation for the robot to reach
-  ROS_INFO("Sending goal to rusty");
   ac.sendGoal(goal);
 
-  // Wait an infinite time for the results
   ac.waitForResult();
 
-  // Check if the robot reached its goal
-  if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
-    ROS_INFO("Rusty as reached");
-  else
-    ROS_INFO("Rusty could not reach ");
+  if (ac.getState()==actionlib::SimpleClientGoalState::SUCCEEDED)
+  {
+    delivery_data.data = 1;
+    pub.publish(delivery_data);
+    ROS_INFO("PICK UP REACHED");
+  }
+  else{
+    delivery_data.data = 2;
+    pub.publish(delivery_data);
+    ROS_INFO("Rusty did not reach the location");
+  }
 
-  return 0;
-}
+  //DROP Sequence
+  ros::Duration(5).sleep();
 
+  n.getParam("/drop/x", goal.target_pose.pose.position.x);
+  n.getParam("/drop/w", goal.target_pose.pose.orientation.w);
 
+  ac.sendGoal(goal);
 
-int main(int argc, char** argv)
-{
-  // Initialize the simple_navigation_goals node
-  ros::init(argc, argv, "pick_drop");
-  ROS_INFO("PICK GOAL");
-  pose(-6.0);
-  ros::Duration(0.5).sleep();
-  ROS_INFO("DROP GOAL");
-  pose(10.0);
-
-
+  ac.waitForResult();
+
+  if (ac.getState()==actionlib::SimpleClientGoalState::SUCCEEDED)
+  {
+    delivery_data.data = 3;
+    pub.publish(delivery_data);
+    ROS_INFO("PICK UP REACHED");
+  }
+  else{
+    delivery_data.data = 2;
+    pub.publish(delivery_data);
+    ROS_INFO("Rusty did not reach the location");
+  }
   return 0;
-}
+}

rusty/src/virtual_box.cpp

@@ -1,82 +1,121 @@
 #include <ros/ros.h>
 #include <visualization_msgs/Marker.h>
+// #include <nav_msgs/Odometry.h>
+#include <std_msgs/UInt8.h>
 
-#include <iostream>
-#include <nav_msgs/Odometry.h>
+u_int8_t state=0;
 
-
-
-float po;
-void posecallback(const nav_msgs::Odometry::ConstPtr& msg)
+void goalCallback(const std_msgs::UInt8::ConstPtr& msg)
 {
-    po=msg->pose.pose.position.x;
-    std::cout<<"POSITION:"<<po;
+   state = msg->data;
+   return;
 }
 
-int main(int argc,char **argv)
+int main( int argc, char** argv )
 {
-  ros::init(argc, argv, "basic_shapes");
+  ros::init(argc, argv, "add_markers");
+
   ros::NodeHandle n;
-  ros::Rate r(1);
+  ros::Rate r(5);
   ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);
+  ros::Subscriber sub = n.subscribe("/goal", 1, goalCallback);
+  bool service_done = false;
 
-  ros::Subscriber sub=n.subscribe("odom",10,posecallback);
-
+  // Set our initial shape type to be a cube
   uint32_t shape = visualization_msgs::Marker::CUBE;
-  float position_arr[2]={-6.0,10.0};
-  for (int i=0;i<3;i++)
-  {
-    if(po<=position_arr[i])
+
+  ROS_INFO("Subscribed to desired goal-position");
+
+  while (ros::ok()) {
+
+    ros::spinOnce();
+    visualization_msgs::Marker marker;
+
+    marker.header.frame_id = "/map";
+    marker.header.stamp = ros::Time::now();
+
+
+    marker.ns = "Delivery_box";
+    marker.id = 0;
+
+    marker.type = shape;
+
+    marker.scale.x = 0.5;
+    marker.scale.y = 0.5;
+    marker.scale.z = 0.5;
+
+
+
+    marker.color.r = 0.0f;
+    marker.color.g = 0.0f;
+    marker.color.b = 1.0f;
+    marker.color.a = 1.0;
+
+    switch (state)
     {
-       while (ros::ok())
+      case 0: 
         {
-          visualization_msgs::Marker marker;
 
-          marker.header.frame_id = "/map";
-          marker.header.stamp = ros::Time::now();
-          marker.ns = "BOX";
-          marker.id = 0;
-          marker.type = shape;
           marker.action = visualization_msgs::Marker::ADD;
+
+          n.getParam("/pick_up/x", marker.pose.position.x);
+
+          n.getParam("/pick_up/w", marker.pose.orientation.w);
+          break;
+        } 
 
-          marker.pose.position.x = po;
-          ROS_INFO("POSITION:%f",po);
-          marker.pose.position.y = 0;
-          marker.pose.position.z = 0;
-          marker.pose.orientation.x = 0.0;
-          marker.pose.orientation.y = 0.0;
-          marker.pose.orientation.z = 0.0;
-          marker.pose.orientation.w = 1.0;
-
-          marker.scale.x = 3.0;
-          marker.scale.y = 3.0;
-          marker.scale.z = 2.0;
-
-          marker.color.r = 0.0f;
-          marker.color.g = 1.0f;
-          marker.color.b = 0.0f;
-          marker.color.a = 1.0;
-          marker.lifetime = ros::Duration(20);
-
-          while (marker_pub.getNumSubscribers() >= 1)
+        case 1:   
           {
-            if (!ros::ok())
-            {
-              return 0;
-            }
-            ROS_WARN_ONCE("Please create a subscriber to the marker");
-            sleep(1);
+            sleep(2);
+
+            marker.action = visualization_msgs::Marker::DELETE;
+            break;
+          } 
+
+        case 2: 
+          {
+            marker.action = visualization_msgs::Marker::DELETE;
+            break;
+          }
+
+        case 3:  
+          {
+            sleep(5);
+
+            marker.action = visualization_msgs::Marker::ADD;
+
+            n.getParam("/drop/x", marker.pose.position.x);
+
+            n.getParam("/drop/w", marker.pose.orientation.w);
+            service_done = true;
+            break;
           }
-          marker_pub.publish(marker);
-          r.sleep();
-        }
-
 
-       ros::Duration(5.0).sleep();
-
+    } 
+
+
+    while (marker_pub.getNumSubscribers() < 1)
+    {
+      if (!ros::ok())
+      {
+        return 0;
+      }
+      ROS_WARN("Please create a subscriber to the marker");
+      sleep(1);
     }
-  }
-
-
-
-}
+
+
+    marker_pub.publish(marker);
+
+
+    if (service_done) {
+      ROS_INFO(" Reached");
+      sleep(7);
+      return 0;
+      }
+
+    r.sleep();
+  } 
+
+  return 0;
+}